Agrarsense
Public Member Functions | Protected Member Functions | Private Member Functions | Private Attributes | Friends | List of all members
ALidar Class Reference

#include <Lidar.h>

Inheritance diagram for ALidar:
Inheritance graph
[legend]
Collaboration diagram for ALidar:
Collaboration graph
[legend]

Public Member Functions

 ALidar (const FObjectInitializer &ObjectInitializer)
 
void Init (FLidarParameters parameters, bool SimulateSensor=true)
 
virtual ESensorTypes GetSensorType () const override
 
void ChangeLidarParameters (FLidarParameters newLidarParameters)
 
void SetVisualizeLidarParticles (bool Visualize)
 
void SetParticleLifeTime (float ParticleLifeTime)
 
FLidarParameters GetLidarParameters () const
 
UNiagaraComponent * GetNiagaraComponent () const
 
virtual FString GetParametersAsString () const override
 
- Public Member Functions inherited from ASensor
 ASensor (const FObjectInitializer &ObjectInitializer)
 
FString ExportToJsonFile (const FString &FileName)
 
virtual ESensorTypes GetSensorType () const
 
FString GetSensorIdentifier () const
 
void SetSensorIdentifier (const FString newIdentifier)
 
FString GetSensorName () const
 
virtual FString GetParametersAsString () const
 
void SetSensorName (const FString newName)
 
virtual FString GetTopicName ()
 
UTopic * GetROSTopic () const
 
void SetSimulateSensor (bool SimulateSensor)
 
bool CanSimulateSensor () const
 
ASensorModelGetSensorModel () const
 
void SetSensorModel (ASensorModel *NewSensorModel)
 
FORCEINLINE bool IsROSConnected () const
 
UROSIntegrationGameInstance * GetROSGameInstance () const
 
virtual FString GetActorID_Implementation () const override
 
virtual FString GetActorName_Implementation () const override
 
virtual FString GetActorInformation_Implementation () const override
 
virtual void SetActorName_Implementation (const FString &NewActorName) override
 
virtual void SetActorIDAndName_Implementation (const FString &NewActorName, const FString &NewID) override
 
- Public Member Functions inherited from IActorInformation
FString GetActorID () const
 
FString GetActorName () const
 
FString GetActorInformation () const
 
void SetActorName (const FString &NewActorName)
 
void SetActorIDAndName (const FString &NewActorName, const FString &NewID)
 

Protected Member Functions

virtual void BeginPlay () override
 
virtual void EndPlay (const EEndPlayReason::Type EndPlayReason) override
 
- Protected Member Functions inherited from ASensor
virtual void BeginPlay () override
 
virtual void EndPlay (const EEndPlayReason::Type EndPlayReason) override
 
virtual void CreateROSTopic ()
 
virtual void DestroyROSTopic ()
 
virtual void CreateDataSavePath ()
 
bool IsLogFileCreated ()
 
virtual void CreateLogFile ()
 
void WriteToLogFile (const FString &Message)
 

Private Member Functions

std::vector< FPointDataSimulateRaycastLidar (const float DeltaTime)
 
void SetNiagaraRendering (bool Enabled)
 
void VisualizeLidarParticles (std::vector< FPointData > HitPoints)
 
void ChangeParameters ()
 
FORCEINLINE bool ShootLaser (const UWorld *World, FHitResult &HitResult, const FVector &EndTrace, const FCollisionQueryParams &TraceParams, const FVector &LidarBodyLocation, const int32 Channel, const bool Semantic, const uint32 RGBDefault, const bool UseLidarNoiseModel)
 
void ResetRecordedHits (int32 MaxPointsPerChannel)
 
FORCEINLINE uint32 GetSemanticData (const FHitResult &HitResult) const
 
void SendData (const float DeltaTime)
 
bool CanSendData (const float DeltaTime)
 
void SendDataToTopic (const std::vector< FPointData > &points)
 
void SaveDataToDisk ()
 
void AddProcessingToHitResult (FHitResult &HitResult, const FVector &LidarBodyLocation, const bool UseHorizontalNoise, const bool UseSnowTerrainAdjustment)
 
void AddDistanceNoise (FHitResult &HitResult, const FVector &LidarBodyLocation)
 
void AddLateralNoise (FHitResult &HitResult, const FVector &LidarBodyLocation, const bool UseHorizontalNoise)
 
void AddSnowTerrainAdjustment (FHitResult &HitResult, const FVector &LidarBodyLocation)
 
void UpdateLidarParticles (int32 NumberOfHits, const TArray< FVector > &HitPoints, const TArray< FLinearColor > &Colors)
 
void OnWeatherChanged (FWeatherParameters WeatherParams)
 

Private Attributes

UNiagaraComponent * NiagaraComponent = nullptr
 
FLidarParameters LidarParameters
 
FLidarParameters TempLidarParameters
 
FWeatherParameters CurrentWeatherParameters
 
TMap< FName, FColor > SemanticColors
 
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > PointCloudMessage
 
std::vector< std::vector< FPointData > > PreProcessedHitImpactPoints
 
std::vector< std::vector< FPointData > > Points
 
std::vector< FPointDataPointsFlattened
 
std::vector< float > LaserAngles
 
std::random_device RandomDevice
 
std::mt19937 Generator
 
bool NeedToSavePointCloudWithoutNoise = false
 
bool LidarParametersChanged = false
 
bool SendDataEveryFrame = false
 
bool ClearContainers = true
 
const FName TerrainTag = "Terrain"
 
float CurrentHorizontalAngle = 0.0f
 
float SensorHzFrequency = 0.1f
 
float SensorHzTimer = 0.0f
 
float SnowHeightOffset = 0.0f
 
int32 LidarSaves = 0
 
uint32 CachedNoneColor
 
uint32 CachedSnowflakeColor
 

Friends

class ALidarManager
 

Additional Inherited Members

- Static Public Member Functions inherited from ASensor
static void HideComponentForAllCameras (UPrimitiveComponent *PrimitiveComponent)
 
static TMap< FString, FColor > GetSemanticColors ()
 
static TArray< TWeakObjectPtr< UPrimitiveComponent > > GetComponentsToHide ()
 
- Static Public Member Functions inherited from IActorInformation
static void SetAndValidateActorIDAndName (FString &ActorName, FString &ActorID, TWeakObjectPtr< AActor > Actor)
 
static bool DestroyActorByID (const FString &ID)
 
static AActor * GetActorByID (const FString &ID)
 
template<typename T >
static TArray< T * > GetActorsWithInterface ()
 
static void PrintAllIds ()
 
- Public Attributes inherited from ASensor
FSensorDestroy OnSensorDestroy
 
FString AttachedToComponent
 
FName AttachedToBone
 
- Static Protected Member Functions inherited from ASensor
template<typename InStructType >
static FString StructToString (const InStructType &InStruct)
 
- Protected Attributes inherited from ASensor
UTopic * ROSTopic = nullptr
 
bool SendDataToROS = true
 
ULogFileLogFile = nullptr
 
FString FileSavePath
 
UROSIntegrationGameInstance * ROSInstance = nullptr
 
- Static Protected Attributes inherited from ASensor
static FPrimitiveAdded OnPrimitiveAdded
 
static const FName NiagaraPointsInt = "User.PointCount"
 
static const FName NiagaraHitPoints = "User.HitPoints"
 
static const FName NiagaraHitColors = "User.HitColors"
 
static const FName NiagaraPointsFloat = "User.Test"
 

Detailed Description

Parameterized linetrace based Lidar sensor. Parameters defined in FLidarParameters struct. This Sensor Actor is ticked by LidarManager where all added Lidar sensors are ticked in parallel.

Definition at line 33 of file Lidar.h.

Constructor & Destructor Documentation

◆ ALidar()

ALidar::ALidar ( const FObjectInitializer &  ObjectInitializer)

Definition at line 32 of file Lidar.cpp.

32 : Super(ObjectInitializer)
33{
34 // Lidar sensors are ticked by LidarManager.cpp
35 PrimaryActorTick.bCanEverTick = false;
36}

Member Function Documentation

◆ AddDistanceNoise()

void ALidar::AddDistanceNoise ( FHitResult &  HitResult,
const FVector &  LidarBodyLocation 
)
private

Definition at line 386 of file Lidar.cpp.

387{
388 std::normal_distribution<float> Distribution(0.0f, LidarParameters.DistanceNoiseStdDev);
389 float NoiseValue = Distribution(Generator);
390 const FVector DirectionToSensor = ((LidarBodyLocation * 1e-2) - HitResult.ImpactPoint).GetSafeNormal();
391 const FVector Noise = DirectionToSensor * NoiseValue;
392 HitResult.ImpactPoint += Noise;
393}
std::mt19937 Generator
Definition: Lidar.h:251
FLidarParameters LidarParameters
Definition: Lidar.h:232

References FLidarParameters::DistanceNoiseStdDev, Generator, and LidarParameters.

Referenced by AddProcessingToHitResult().

◆ AddLateralNoise()

void ALidar::AddLateralNoise ( FHitResult &  HitResult,
const FVector &  LidarBodyLocation,
const bool  UseHorizontalNoise 
)
private

Definition at line 395 of file Lidar.cpp.

396{
397 const FVector ForwardVector = (HitResult.ImpactPoint - LidarBodyLocation).GetSafeNormal();
398 const FVector RightVector = FVector::CrossProduct(ForwardVector, FVector::UpVector).GetSafeNormal();
399
400 FVector ChosenDirection;
401 if (UseHorizontalNoise)
402 {
403 ChosenDirection = RightVector;
404 }
405 else
406 {
407 FVector UpVector = FVector::CrossProduct(ForwardVector, RightVector).GetSafeNormal();
408 ChosenDirection = UpVector;
409 }
410
411 std::normal_distribution<float> Distribution(0.0f, LidarParameters.LateralNoiseStdDev);
412 const float NoiseValue = Distribution(Generator);
413
414 HitResult.ImpactPoint += ChosenDirection * NoiseValue;
415}

References Generator, FLidarParameters::LateralNoiseStdDev, and LidarParameters.

Referenced by AddProcessingToHitResult().

◆ AddProcessingToHitResult()

void ALidar::AddProcessingToHitResult ( FHitResult &  HitResult,
const FVector &  LidarBodyLocation,
const bool  UseHorizontalNoise,
const bool  UseSnowTerrainAdjustment 
)
private

Definition at line 368 of file Lidar.cpp.

369{
370 if (UseSnowTerrainAdjustment)
371 {
372 AddSnowTerrainAdjustment(HitResult, LidarBodyLocation);
373 }
374
376 {
377 AddDistanceNoise(HitResult, LidarBodyLocation);
378 }
379
381 {
382 AddLateralNoise(HitResult, LidarBodyLocation, UseHorizontalNoise);
383 }
384}
void AddLateralNoise(FHitResult &HitResult, const FVector &LidarBodyLocation, const bool UseHorizontalNoise)
Definition: Lidar.cpp:395
void AddDistanceNoise(FHitResult &HitResult, const FVector &LidarBodyLocation)
Definition: Lidar.cpp:386
void AddSnowTerrainAdjustment(FHitResult &HitResult, const FVector &LidarBodyLocation)
Definition: Lidar.cpp:417

References AddDistanceNoise(), AddLateralNoise(), AddSnowTerrainAdjustment(), FLidarParameters::DistanceNoiseStdDev, FLidarParameters::LateralNoiseStdDev, and LidarParameters.

Referenced by SimulateRaycastLidar().

◆ AddSnowTerrainAdjustment()

void ALidar::AddSnowTerrainAdjustment ( FHitResult &  HitResult,
const FVector &  LidarBodyLocation 
)
private

Definition at line 417 of file Lidar.cpp.

418{
419 AActor* HitActor = HitResult.GetActor();
420
421 // Terrain should have tag "Terrain" in order to make this work.
422 if (HitActor && HitActor->Tags.Contains(TerrainTag))
423 {
424 // Scale the Z value of ImpactPoint based on SnowHeightOffset.
425 HitResult.ImpactPoint.Z += SnowHeightOffset;
426 }
427}
const FName TerrainTag
Definition: Lidar.h:258
float SnowHeightOffset
Definition: Lidar.h:264

References SnowHeightOffset, and TerrainTag.

Referenced by AddProcessingToHitResult().

◆ BeginPlay()

void ALidar::BeginPlay ( )
overrideprotectedvirtual

Reimplemented from ASensor.

Definition at line 53 of file Lidar.cpp.

54{
55 Super::BeginPlay();
56
57 UWorld* World = GetWorld();
58
59 // Create the SemanticColors TMap
60 TMap<FString, FColor> Colors = GetSemanticColors();
61 for (const auto& ColorEntry : Colors)
62 {
63 FName SemanticName = FName(*ColorEntry.Key);
64 SemanticColors.Add(SemanticName, ColorEntry.Value);
65 }
66
67 // Cache "None" and "Snowflake" colors for Semantic
68 FColor NoneColor = SemanticColors.FindRef("None");
69 FColor SnowflakeColor = SemanticColors.FindRef("Snowflake");
70 CachedNoneColor = (NoneColor.R << 16) | (NoneColor.G << 8) | (NoneColor.B << 0);
71 CachedSnowflakeColor = (SnowflakeColor.R << 16) | (SnowflakeColor.G << 8) | (SnowflakeColor.B << 0);
72
73 // Add this lidar to LidarManager that handles ticking of this sensor in parallel.
75 if (LidarManager)
76 {
77 LidarManager->AddRaycastLidar(this);
78 }
79
81 if (Weather)
82 {
84 Weather->OnWeatherChanged.AddUniqueDynamic(this, &ALidar::OnWeatherChanged);
85 }
86
87 // Load pointcloud Niagara particle system
88 UNiagaraSystem* NS = LoadObject<UNiagaraSystem>(nullptr, TEXT("/Game/Agrarsense/Particles/PointCloud/NS_PointCloudSemanticFast"), nullptr, LOAD_None, nullptr);
89 if (NS)
90 {
91 NiagaraComponent = UNiagaraFunctionLibrary::SpawnSystemAtLocation(World, NS, GetActorLocation());
93 {
94 // Hide this NiagaraComponent for all Camera sensors.
95 HideComponentForAllCameras(Cast<UPrimitiveComponent>(NiagaraComponent));
96
97 NiagaraComponent->SetAffectDynamicIndirectLighting(false);
98 NiagaraComponent->SetAffectDistanceFieldLighting(false);
99 NiagaraComponent->SetCastContactShadow(false);
100 NiagaraComponent->SetCastShadow(false);
101
103 }
104 }
105#if WITH_EDITOR
106 else
107 {
108 UE_LOG(LogTemp, Warning, TEXT("Lidar.cpp: Couldn't find Niagara particle system."));
109 }
110#endif
111}
void AddRaycastLidar(ALidar *lidarPtr)
UNiagaraComponent * NiagaraComponent
Definition: Lidar.h:126
TMap< FName, FColor > SemanticColors
Definition: Lidar.h:239
FWeatherParameters CurrentWeatherParameters
Definition: Lidar.h:236
uint32 CachedSnowflakeColor
Definition: Lidar.h:270
void SetParticleLifeTime(float ParticleLifeTime)
Definition: Lidar.cpp:236
void OnWeatherChanged(FWeatherParameters WeatherParams)
Definition: Lidar.cpp:150
uint32 CachedNoneColor
Definition: Lidar.h:268
static void HideComponentForAllCameras(UPrimitiveComponent *PrimitiveComponent)
Definition: Sensor.cpp:282
static TMap< FString, FColor > GetSemanticColors()
Definition: Sensor.cpp:292
const FWeatherParameters & GetCurrentWeather() const
Definition: Weather.h:76
FLevelEventDelegate_WeatherChanged OnWeatherChanged
Definition: Weather.h:85
static AWeather * GetWeatherActor(const UObject *WorldContextObject)
static ALidarManager * GetLidarManager(const UObject *WorldContextObject)

References ALidarManager::AddRaycastLidar(), CachedNoneColor, CachedSnowflakeColor, CurrentWeatherParameters, AWeather::GetCurrentWeather(), UAgrarsenseStatics::GetLidarManager(), ASensor::GetSemanticColors(), UAgrarsenseStatics::GetWeatherActor(), ASensor::HideComponentForAllCameras(), NiagaraComponent, OnWeatherChanged(), AWeather::OnWeatherChanged, SemanticColors, and SetParticleLifeTime().

◆ CanSendData()

bool ALidar::CanSendData ( const float  DeltaTime)
private

Checks can sensor send data in this tick

Definition at line 518 of file Lidar.cpp.

519{
521 {
522 ClearContainers = true;
523 return true;
524 }
525
526 SensorHzTimer += DeltaTime;
528 {
529 SensorHzTimer = 0.0f;
530 ClearContainers = true;
531 return true;
532 }
533 else
534 {
535 ClearContainers = false;
536 return false;
537 }
538}
float SensorHzFrequency
Definition: Lidar.h:261
bool ClearContainers
Definition: Lidar.h:256
bool SendDataEveryFrame
Definition: Lidar.h:255
float SensorHzTimer
Definition: Lidar.h:262

References ClearContainers, SendDataEveryFrame, SensorHzFrequency, and SensorHzTimer.

Referenced by SendData().

◆ ChangeLidarParameters()

void ALidar::ChangeLidarParameters ( FLidarParameters  newLidarParameters)

Change Lidar parameter for this sensor.

Parameters
newLidarParametersFLidarParameters struct

Definition at line 540 of file Lidar.cpp.

541{
542 if (!CanSimulateSensor())
543 {
544 // We are not currently simulating sensor, we can safely change parameters here
545 LidarParameters = newLidarParameters;
547 }
548 else
549 {
550 // Change parameters at the end of the frame
551 TempLidarParameters = newLidarParameters;
553 }
554}
bool LidarParametersChanged
Definition: Lidar.h:254
void ChangeParameters()
Definition: Lidar.cpp:162
FLidarParameters TempLidarParameters
Definition: Lidar.h:234
bool CanSimulateSensor() const
Definition: Sensor.h:161

References ASensor::CanSimulateSensor(), ChangeParameters(), LidarParameters, LidarParametersChanged, and TempLidarParameters.

◆ ChangeParameters()

void ALidar::ChangeParameters ( )
private

Change this Lidar sensor parameters

Definition at line 162 of file Lidar.cpp.

163{
164 // Ensure certain parameters are valid ranges
165 LidarParameters.HorizontalFov = FMath::Clamp(LidarParameters.HorizontalFov, 0.1f, 360.0f);
169
170 const int32 NumberOfLasers = LidarParameters.Channels;
171
172 // Resize arrays
173 PreProcessedHitImpactPoints.resize(NumberOfLasers);
174 Points.resize(NumberOfLasers);
175
176 // Create Lidar linetrace angles
177 LaserAngles.clear();
178 const float DeltaAngle = NumberOfLasers == 1u ? 0.f : (LidarParameters.UpperFovLimit - LidarParameters.LowerFovLimit) / static_cast<float>(NumberOfLasers - 1);
179
180 for (int32 i = 0; i < NumberOfLasers; ++i)
181 {
182 const float VerticalAngle = LidarParameters.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
183 LaserAngles.emplace_back(VerticalAngle);
184 }
185
189
190 if (LidarParameters.PointsPerSecond > 1000000)
191 {
192 SimulatorLog::Log("Lidar sensor: It's highly advisable to turn off VisualizePointcloud when PointsPerSecond exceed 1 million.");
193
194 if (LidarParameters.PointsPerSecond > 2000000)
195 {
197 SimulatorLog::Log("Lidar sensor: VisualizePointcloud has been set to false due PointsPerSecond exceeding 2 million.");
198 }
199 }
200
203}
std::vector< std::vector< FPointData > > PreProcessedHitImpactPoints
Definition: Lidar.h:243
std::vector< float > LaserAngles
Definition: Lidar.h:248
std::vector< std::vector< FPointData > > Points
Definition: Lidar.h:245
void SetVisualizeLidarParticles(bool Visualize)
Definition: Lidar.cpp:205
bool SendDataToROS
Definition: Sensor.h:344
virtual void CreateROSTopic()
Definition: Sensor.cpp:173
static void Log(const FString &Message, bool LogToTextFile=true, bool LogToROS=true)
bool SendDataAtRotationFrequency

References FLidarParameters::Channels, ASensor::CreateROSTopic(), FLidarParameters::DistanceNoiseStdDev, FLidarParameters::HorizontalFov, LaserAngles, FLidarParameters::LateralNoiseStdDev, LidarParameters, SimulatorLog::Log(), FLidarParameters::LowerFovLimit, Points, FLidarParameters::PointsPerSecond, PreProcessedHitImpactPoints, FLidarParameters::RotationFrequency, FLidarParameters::SendDataAtRotationFrequency, SendDataEveryFrame, FLidarParameters::SendDataToROS, ASensor::SendDataToROS, SensorHzFrequency, SetVisualizeLidarParticles(), FLidarParameters::UpperFovLimit, and FLidarParameters::VisualizePointcloud.

Referenced by ChangeLidarParameters(), Init(), and SimulateRaycastLidar().

◆ EndPlay()

void ALidar::EndPlay ( const EEndPlayReason::Type  EndPlayReason)
overrideprotectedvirtual

Reimplemented from ASensor.

Definition at line 113 of file Lidar.cpp.

114{
115 Super::EndPlay(EndPlayReason);
116
117 UWorld* World = GetWorld();
118
119 // Remove this lidar from LidarManager
121 if (LidarManager)
122 {
123 LidarManager->RemoveRaycastLidar(this);
124 }
125
126 // Unbind from Weather changed event
128 if (Weather)
129 {
130 Weather->OnWeatherChanged.RemoveDynamic(this, &ALidar::OnWeatherChanged);
131 }
132
134 {
135 SetNiagaraRendering(false);
136
137 // Destroy component
138 NiagaraComponent->UnregisterComponent();
139 NiagaraComponent->DestroyComponent();
140 NiagaraComponent = nullptr;
141 }
142
143 PointCloudMessage.Reset();
145 PointsFlattened.clear();
146 LaserAngles.clear();
147 Points.clear();
148}
void RemoveRaycastLidar(ALidar *lidarPtr)
void SetNiagaraRendering(bool Enabled)
Definition: Lidar.cpp:210
std::vector< FPointData > PointsFlattened
Definition: Lidar.h:246
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > PointCloudMessage
Definition: Lidar.h:241

References UAgrarsenseStatics::GetLidarManager(), UAgrarsenseStatics::GetWeatherActor(), LaserAngles, NiagaraComponent, OnWeatherChanged(), AWeather::OnWeatherChanged, PointCloudMessage, Points, PointsFlattened, PreProcessedHitImpactPoints, ALidarManager::RemoveRaycastLidar(), and SetNiagaraRendering().

◆ GetLidarParameters()

FLidarParameters ALidar::GetLidarParameters ( ) const
inline

Get current Lidar parameters

Returns
FLidarParameters struct

Definition at line 82 of file Lidar.h.

83 {
84 return LidarParameters;
85 }

Referenced by USimulatorJsonExporter::ExportSensorToJSON().

◆ GetNiagaraComponent()

UNiagaraComponent * ALidar::GetNiagaraComponent ( ) const
inline

Retrieves the Niagara component associated with this sensor.

Returns
A pointer to the UNiagaraComponent instance associated with this sensor.

Definition at line 92 of file Lidar.h.

93 {
94 return NiagaraComponent;
95 }

◆ GetParametersAsString()

virtual FString ALidar::GetParametersAsString ( ) const
inlineoverridevirtual

Get current LidarParameters struct fields as one string.

Reimplemented from ASensor.

Definition at line 100 of file Lidar.h.

101 {
103 }
static FString StructToString(const InStructType &InStruct)
Definition: Sensor.h:305

◆ GetSemanticData()

uint32 ALidar::GetSemanticData ( const FHitResult &  HitResult) const
private

Retrieve semantic data based on the object hit by the line trace.

Parameters
HitResultThe result of the line trace indicating the object hit.
Returns
The semantic data associated with the hit object.

Definition at line 556 of file Lidar.cpp.

557{
558 auto* HitComponent = HitResult.Component.Get();
559
560 if (HitComponent)
561 {
562 TArray<FName>& CompTags = HitComponent->ComponentTags;
563 if (!CompTags.IsEmpty())
564 {
565 FName Tag = CompTags[0]; // First component tag should be the tag we want.
566 FColor Color = SemanticColors.FindRef(Tag);
567 return (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
568 }
569 else
570 {
571 return CachedNoneColor;
572 }
573 }
574 else
575 {
577 }
578}

References CachedNoneColor, CachedSnowflakeColor, and SemanticColors.

Referenced by ShootLaser(), and SimulateRaycastLidar().

◆ GetSensorType()

virtual ESensorTypes ALidar::GetSensorType ( ) const
inlineoverridevirtual

Get type of the sensor

Returns
Sensor's type

Reimplemented from ASensor.

Definition at line 51 of file Lidar.h.

52 {
54 }

References Lidar.

◆ Init()

void ALidar::Init ( FLidarParameters  parameters,
bool  SimulateSensor = true 
)

Initialize new Lidar sensor

Parameters
parametersFLidarParameters struct

Definition at line 38 of file Lidar.cpp.

39{
40 LidarParameters = parameters;
41 SetSimulateSensor(SimulateSensor);
44
45 // Initialize PointCloudMessage
46 PointCloudMessage.Reset();
47 PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
48 PointCloudMessage->CreateDefaultPointCloud2Message();
49
50 Generator.seed(RandomDevice());
51}
std::random_device RandomDevice
Definition: Lidar.h:250
virtual void CreateDataSavePath()
Definition: Sensor.cpp:229
void SetSimulateSensor(bool SimulateSensor)
Definition: Sensor.h:151

References ChangeParameters(), ASensor::CreateDataSavePath(), Generator, LidarParameters, PointCloudMessage, RandomDevice, and ASensor::SetSimulateSensor().

Referenced by USensorFactory::SpawnLidarSensor().

◆ OnWeatherChanged()

void ALidar::OnWeatherChanged ( FWeatherParameters  WeatherParams)
private

Definition at line 150 of file Lidar.cpp.

151{
152 CurrentWeatherParameters = WeatherParams;
153
154 // Here we recalculate SnowHeightOffset when Weather changes.
155 // This is fine-tuned by hand. If the Snow implementation would change meaningfully,
156 // we would need to change this implementation as well.
157 // See AddSnowTerrainAdjustment() function how SnowHeightOffset is used.
159 SnowHeightOffset *= 1e-2;
160}

References CurrentWeatherParameters, FWeatherParameters::SnowAmount, and SnowHeightOffset.

Referenced by BeginPlay(), and EndPlay().

◆ ResetRecordedHits()

void ALidar::ResetRecordedHits ( int32  MaxPointsPerChannel)
private

Reset and possibly re-allocate needed arrays.

Definition at line 641 of file Lidar.cpp.

642{
643 if (!ClearContainers)
644 {
645 return;
646 }
647
648 // Visualize Lidar particles on main thread because,
649 // in some instances NiagaraComponent crashes when updating points from background thread.
650 std::vector<FPointData> CopiedPoints = PointsFlattened;
651 AsyncTask(ENamedThreads::GameThread, [this, CopiedPoints]()
652 {
653 VisualizeLidarParticles(CopiedPoints);
654 });
655
657 {
658 // If we only send data at Lidar sensor rotation frequency
659 // reserve enough points to fill all points
661 }
662
663 // Clear all containers
665 {
666 for (std::vector<FPointData>& ImpactPoints : PreProcessedHitImpactPoints)
667 {
668 ImpactPoints.clear();
669 ImpactPoints.reserve(MaxPointsPerChannel);
670 }
671 }
672
673 for (std::vector<FPointData>& ImpactPoints : Points)
674 {
675 ImpactPoints.clear();
676 ImpactPoints.reserve(MaxPointsPerChannel);
677 }
678
679 PointsFlattened.clear();
680}
void VisualizeLidarParticles(std::vector< FPointData > HitPoints)
Definition: Lidar.cpp:429
bool SavePointcloudWithoutNoiseModel

References FLidarParameters::Channels, ClearContainers, LidarParameters, Points, PointsFlattened, FLidarParameters::PointsPerSecond, PreProcessedHitImpactPoints, FLidarParameters::SavePointcloudWithoutNoiseModel, SendDataEveryFrame, and VisualizeLidarParticles().

Referenced by SimulateRaycastLidar().

◆ SaveDataToDisk()

void ALidar::SaveDataToDisk ( )
private

Definition at line 704 of file Lidar.cpp.

705{
707 {
708 return;
709 }
710
711 // Save point cloud to disk
712 FString Filename = FString::Printf(TEXT("%sLidar_%d.ply"), *FileSavePath, LidarSaves);
714
716 {
717 // Flatten 2D std::vector to 1D std::vector
718 std::vector<FPointData> PointsWithOutNoiseModel;
719
720 size_t totalSize = 0;
721 for (const std::vector<FPointData>& Vec : PreProcessedHitImpactPoints)
722 {
723 totalSize += Vec.size();
724 }
725 PointsWithOutNoiseModel.reserve(totalSize);
726
727 for (const std::vector<FPointData>& Vec : PreProcessedHitImpactPoints)
728 {
729 PointsWithOutNoiseModel.insert(PointsWithOutNoiseModel.end(), Vec.begin(), Vec.end());
730 }
731
732 // Save pointcloud to disk without any noise
733 FString PreprocessedFilename = FString::Printf(TEXT("%sLidar_%d_without_noise_model.ply"), *FileSavePath, LidarSaves);
734 UPointcloudUtilities::SaveVectorArrayAsPlyAsync(PreprocessedFilename, PointsWithOutNoiseModel);
735 }
736
737 ++LidarSaves;
738}
int32 LidarSaves
Definition: Lidar.h:266
bool NeedToSavePointCloudWithoutNoise
Definition: Lidar.h:253
FString FileSavePath
Definition: Sensor.h:349
static void SaveVectorArrayAsPlyAsync(FString FullFileName, const std::vector< FPointData > points)

References ASensor::FileSavePath, LidarParameters, LidarSaves, NeedToSavePointCloudWithoutNoise, PointsFlattened, PreProcessedHitImpactPoints, FLidarParameters::SaveDataToDisk, and UPointcloudUtilities::SaveVectorArrayAsPlyAsync().

Referenced by SendData().

◆ SendData()

void ALidar::SendData ( const float  DeltaTime)
private

Send data to topics and save point cloud to disk if that's enabled. Before sending and saving, CanSendData is checked.

Definition at line 486 of file Lidar.cpp.

487{
488 if (!CanSendData(DeltaTime))
489 {
490 return;
491 }
492
493 // Preallocate PointsFlattened size
494 // and fill PointsFlattened
495 size_t TotalSize = 0;
496 for (const std::vector<FPointData>& Vec : Points)
497 {
498 TotalSize += Vec.size();
499 }
500 PointsFlattened.reserve(TotalSize);
501
502 for (std::vector<FPointData>& HitPoints : Points)
503 {
504 PointsFlattened.insert(PointsFlattened.end(), std::make_move_iterator(HitPoints.begin()), std::make_move_iterator(HitPoints.end()));
505 }
506
508 {
510 }
511
512 if (SendDataToROS)
513 {
515 }
516}
bool CanSendData(const float DeltaTime)
Definition: Lidar.cpp:518
void SaveDataToDisk()
Definition: Lidar.cpp:704
void SendDataToTopic(const std::vector< FPointData > &points)
Definition: Lidar.cpp:682

References CanSendData(), LidarParameters, Points, PointsFlattened, SaveDataToDisk(), FLidarParameters::SaveDataToDisk, ASensor::SendDataToROS, and SendDataToTopic().

Referenced by SimulateRaycastLidar().

◆ SendDataToTopic()

void ALidar::SendDataToTopic ( const std::vector< FPointData > &  points)
private

Definition at line 682 of file Lidar.cpp.

683{
684 UTopic* Topic = GetROSTopic();
685
686 if (!IsROSConnected()
687 || !Topic
688 || !SendDataToROS
689 || !PointCloudMessage.IsValid()
690 || points.empty())
691 {
692 return;
693 }
694
695 const size_t PointsSize = points.size();
696 const size_t DataSize = PointsSize * sizeof(FPointData);
697
698 PointCloudMessage->width = static_cast<uint32>(PointsSize);
699 PointCloudMessage->row_step = static_cast<uint32>(DataSize);
700 PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
701 Topic->Publish(PointCloudMessage);
702}
UTopic * GetROSTopic() const
Definition: Sensor.h:141
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:192

References ASensor::GetROSTopic(), ASensor::IsROSConnected(), PointCloudMessage, and ASensor::SendDataToROS.

Referenced by SendData().

◆ SetNiagaraRendering()

void ALidar::SetNiagaraRendering ( bool  Enabled)
private

Set Niagara component rendering on/off

Parameters
bEnabledboolean to set Niagara rendering on/off

Definition at line 210 of file Lidar.cpp.

211{
213 {
214 if (!Enabled)
215 {
216 TArray<FVector> EmptyHitpoints;
217 TArray<FLinearColor> EmptyColors;
218 UpdateLidarParticles(0, EmptyHitpoints, EmptyColors);
219 }
220
221 NiagaraComponent->SetRenderingEnabled(Enabled);
222 }
223}
void UpdateLidarParticles(int32 NumberOfHits, const TArray< FVector > &HitPoints, const TArray< FLinearColor > &Colors)
Definition: Lidar.cpp:225

References NiagaraComponent, and UpdateLidarParticles().

Referenced by EndPlay(), and SetVisualizeLidarParticles().

◆ SetParticleLifeTime()

void ALidar::SetParticleLifeTime ( float  ParticleLifeTime)

Set Niagara particle lifetime (default 2 seconds)

Parameters
particleLifeTimenew lifetime as float

Definition at line 236 of file Lidar.cpp.

237{
239 {
240 NiagaraComponent->SetVariableFloat(FName("User.LifeTime"), ParticleLifeTime);
241 }
242}

References NiagaraComponent.

Referenced by BeginPlay().

◆ SetVisualizeLidarParticles()

void ALidar::SetVisualizeLidarParticles ( bool  Visualize)

Set boolean whether Lidar sensors be visualized

Parameters
bVisualizeboolean to set visualization on/off

Definition at line 205 of file Lidar.cpp.

206{
207 SetNiagaraRendering(Visualize);
208}

References SetNiagaraRendering().

Referenced by ChangeParameters().

◆ ShootLaser()

bool ALidar::ShootLaser ( const UWorld *  World,
FHitResult &  HitResult,
const FVector &  EndTrace,
const FCollisionQueryParams &  TraceParams,
const FVector &  LidarBodyLocation,
const int32  Channel,
const bool  Semantic,
const uint32  RGBDefault,
const bool  UseLidarNoiseModel 
)
private

This function calculates the impact point of the laser shot based on provided parameters, considering the specified vertical and horizontal angles, collision parameters, Lidar body location, Lidar body rotation, and the specified range.

Returns
true if the laser shot hits a valid target, false otherwise.

Definition at line 580 of file Lidar.cpp.

582{
583#ifdef ParallelLineTraceSingleByChannel_EXISTS
584 // Defined and implemented in our AGRARSENSE Unreal Engine fork
585 // This LineTrace method doesn't block the physics scene which improves linetrace performance.
586 World->ParallelLineTraceSingleByChannel(
587 HitResult,
588 LidarBodyLocation,
589 EndTrace,
590 ECC_GameTraceChannel3,
591 TraceParams,
592 FCollisionResponseParams::DefaultResponseParam);
593#else
594 // If not using our fork of the engine, use default LineTrace method.
595 World->LineTraceSingleByChannel(
596 HitResult,
597 LidarBodyLocation,
598 EndTrace,
599 ECC_GameTraceChannel3,
600 TraceParams,
601 FCollisionResponseParams::DefaultResponseParam);
602#endif
603
604 bool BlockingHit = HitResult.IsValidBlockingHit();
605 if (!BlockingHit && !UseLidarNoiseModel)
606 {
607 return false;
608 }
609
610 // Scale the pointcloud
611 HitResult.ImpactPoint *= 1e-2;
612
613 // Flip Y axis
614 HitResult.ImpactPoint.Y *= -1;
615
617 {
618 // Save Original FHitResult to separate array
619 FHitResult OriginalHitResultCopy = HitResult;
620 uint32 RGB = RGBDefault;
621 if (Semantic)
622 {
623 // If Lidar is set to Semantic, set point color
624 RGB = GetSemanticData(OriginalHitResultCopy);
625 }
626 const FVector& HitPoint = OriginalHitResultCopy.ImpactPoint;
627 FPointData point = { HitPoint.X, HitPoint.Y, HitPoint.Z, RGB };
628 PreProcessedHitImpactPoints[Channel].emplace_back(point);
629 }
630
631 if (UseLidarNoiseModel)
632 {
633 // Check whether we "hit" snowflake by using LUAS formula
634 bool HitSnowFlake = LidarNoiseModel::CheckSnowflakeHit(HitResult, EndTrace, LidarBodyLocation, CurrentWeatherParameters);
635 BlockingHit = HitResult.bBlockingHit || HitSnowFlake;
636 }
637
638 return BlockingHit;
639}
FORCEINLINE uint32 GetSemanticData(const FHitResult &HitResult) const
Definition: Lidar.cpp:556
static bool CheckSnowflakeHit(FHitResult &HitInfo, const FVector EndTrace, const FVector LidarLocation, const FWeatherParameters &WeatherParameters)
float X
Definition: PointData.h:15

References LidarNoiseModel::CheckSnowflakeHit(), CurrentWeatherParameters, GetSemanticData(), NeedToSavePointCloudWithoutNoise, PreProcessedHitImpactPoints, and FPointData::X.

Referenced by SimulateRaycastLidar().

◆ SimulateRaycastLidar()

std::vector< FPointData > ALidar::SimulateRaycastLidar ( const float  DeltaTime)
private

Simulate this Lidar sensor. Should only be called by LidarManager.

Parameters
DeltaTimecurrent DeltaTime

Definition at line 244 of file Lidar.cpp.

245{
246 TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::SimulateRaycastLidar);
247
248 std::vector<FPointData> HitPoints;
249
250 if (!CanSimulateSensor())
251 {
252 return HitPoints;
253 }
254
255 // Get sensor position and rotation
256 const FTransform& ActorTransform = GetTransform();
257 const FVector LidarBodyLocation = ActorTransform.GetLocation();
258 const FRotator LidarBodyRotation = ActorTransform.Rotator();
259 const FMatrix LidarRotationMatrix = FRotationMatrix(FQuat(LidarBodyRotation).Rotator());
260
261 // Prepare needed variables for processing
262 const int32 ChannelCount = LidarParameters.Channels;
263 const int32 PointsToScanWithOneChannel = FMath::RoundHalfFromZero(LidarParameters.PointsPerSecond * DeltaTime / float(ChannelCount));
264 const float HorizontalAngle = CurrentHorizontalAngle;
265 const float AngleDistanceOfTick = LidarParameters.RotationFrequency * LidarParameters.HorizontalFov * DeltaTime;
266 const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneChannel;
267 const float Range = LidarParameters.Range * 100; // to meters
268
269 const FColor Color = FColor(255, 255, 255);
270 const int32 RgbDefault = (Color.R << 16) | (Color.G << 8) | (Color.B << 0);
271
272 FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
273 TraceParams.bReturnPhysicalMaterial = false;
274
275 // Note. can become very expensive with high vertice meshes.
276 // Any very high vertice mesh should have simpler collision mesh (Tree for example)
277 TraceParams.bTraceComplex = LidarParameters.UseComplexCollisionTrace;
278
279 // Check if we are going to add any noise
283
284 // Check if separate point cloud should be saved without the noise
285 NeedToSavePointCloudWithoutNoise = ShouldAddNoise
288
289 const bool Semantic = LidarParameters.Semantic;
291 const bool NeedsExtraProcessing = UseSnowTerrainAdjustment || LidarParameters.DistanceNoiseStdDev > 0.0f || LidarParameters.LateralNoiseStdDev > 0.0f;
292 const bool UseLidarNoiseModel = LidarParameters.UseLidarNoiseModel;
293
294 const float HorizontalFOV = LidarParameters.HorizontalFov;
295 const float HorizontalFOVHalf = HorizontalFOV / 2;
296
297 const UWorld* World = GetWorld();
298
299 ResetRecordedHits(PointsToScanWithOneChannel);
300
301 // Loop Lidar channels in Parallel
302 ParallelFor(ChannelCount, [&](int32 Channel)
303 {
304 const float VerticalAngle = LaserAngles[Channel];
305 int32 RGB = RgbDefault;
306 FHitResult HitResult;
307
308 // Generate random bool which we use later in AddLateralNoise
309 const bool UseHorizontalNoise = FMath::RandBool();
310
311 // Cache the reference to Points[idxChannel]
312 std::vector<FPointData>& ChannelPoints = Points[Channel];
313
314 // Loop through this channel linetraces
315 for (int32 i = 0; i < PointsToScanWithOneChannel; i++)
316 {
317 //HitResult.Reset();
318
319 // Calculate the EndTrace point for this linetrace
320 const float LaserHorizontalAngle = std::fmod(HorizontalAngle + AngleDistanceOfLaserMeasure * i, HorizontalFOV) - HorizontalFOVHalf;
321 const float VerticalRad = FMath::DegreesToRadians(VerticalAngle);
322 const float HorizontalRad = FMath::DegreesToRadians(LaserHorizontalAngle);
323
324 const FVector LocalDirection(FMath::Cos(VerticalRad) * FMath::Cos(HorizontalRad),
325 FMath::Cos(VerticalRad) * FMath::Sin(HorizontalRad),
326 FMath::Sin(VerticalRad));
327
328 const FVector EndTrace = LidarBodyLocation + Range * LidarRotationMatrix.TransformVector(LocalDirection);
329
330 if (ShootLaser(World, HitResult, EndTrace, TraceParams, LidarBodyLocation, Channel, Semantic, RgbDefault, UseLidarNoiseModel))
331 {
332 if (Semantic)
333 {
334 RGB = GetSemanticData(HitResult);
335 }
336
337 if (NeedsExtraProcessing)
338 {
339 AddProcessingToHitResult(HitResult, LidarBodyLocation, UseHorizontalNoise, UseSnowTerrainAdjustment);
340 }
341
342 // Add HitResult impact point to the current channel's point list
343 const FVector& HitPoint = HitResult.ImpactPoint;
344 ChannelPoints.emplace_back(HitPoint.X, HitPoint.Y, HitPoint.Z, RGB);
345 }
346 }
347 }, EParallelForFlags::Unbalanced);
348
349 CurrentHorizontalAngle = FMath::Fmod(HorizontalAngle + AngleDistanceOfTick, LidarParameters.HorizontalFov);
350
351 SendData(DeltaTime);
352
354 {
358 }
359
361 {
362 return PointsFlattened;
363 }
364
365 return HitPoints;
366}
FORCEINLINE bool ShootLaser(const UWorld *World, FHitResult &HitResult, const FVector &EndTrace, const FCollisionQueryParams &TraceParams, const FVector &LidarBodyLocation, const int32 Channel, const bool Semantic, const uint32 RGBDefault, const bool UseLidarNoiseModel)
Definition: Lidar.cpp:580
void SendData(const float DeltaTime)
Definition: Lidar.cpp:486
void ResetRecordedHits(int32 MaxPointsPerChannel)
Definition: Lidar.cpp:641
std::vector< FPointData > SimulateRaycastLidar(const float DeltaTime)
Definition: Lidar.cpp:244
void AddProcessingToHitResult(FHitResult &HitResult, const FVector &LidarBodyLocation, const bool UseHorizontalNoise, const bool UseSnowTerrainAdjustment)
Definition: Lidar.cpp:368
float CurrentHorizontalAngle
Definition: Lidar.h:260
bool UseTerrainSnowHitAdjustment

References AddProcessingToHitResult(), ASensor::CanSimulateSensor(), ChangeParameters(), FLidarParameters::Channels, ClearContainers, CurrentHorizontalAngle, CurrentWeatherParameters, FLidarParameters::DistanceNoiseStdDev, GetSemanticData(), FLidarParameters::HorizontalFov, FWeatherParameters::IsLidarNoiseModelCondition(), FWeatherParameters::IsWinterSnowCondition(), LaserAngles, FLidarParameters::LateralNoiseStdDev, LidarParameters, LidarParametersChanged, NeedToSavePointCloudWithoutNoise, Points, PointsFlattened, FLidarParameters::PointsPerSecond, FLidarParameters::Range, ResetRecordedHits(), FLidarParameters::RotationFrequency, FLidarParameters::SaveDataToDisk, FLidarParameters::SavePointcloudWithoutNoiseModel, FLidarParameters::Semantic, SendData(), FLidarParameters::SendDataToCombinedROSTopic, ShootLaser(), SimulateRaycastLidar(), TempLidarParameters, FLidarParameters::UseComplexCollisionTrace, FLidarParameters::UseLidarNoiseModel, and FLidarParameters::UseTerrainSnowHitAdjustment.

Referenced by ALidarManager::SimulateLidars(), and SimulateRaycastLidar().

◆ UpdateLidarParticles()

void ALidar::UpdateLidarParticles ( int32  NumberOfHits,
const TArray< FVector > &  HitPoints,
const TArray< FLinearColor > &  Colors 
)
private

Definition at line 225 of file Lidar.cpp.

226{
228 {
229 NiagaraComponent->SetVariableFloat(NiagaraPointsFloat, float(NumberOfHits));
230 NiagaraComponent->SetVariableInt(NiagaraPointsInt, NumberOfHits);
231 UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayVector(NiagaraComponent, NiagaraHitPoints, HitPoints);
232 UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayColor(NiagaraComponent, NiagaraHitColors, Colors);
233 }
234}
static const FName NiagaraHitPoints
Definition: Sensor.h:358
static const FName NiagaraPointsFloat
Definition: Sensor.h:360
static const FName NiagaraHitColors
Definition: Sensor.h:359
static const FName NiagaraPointsInt
Definition: Sensor.h:357

References NiagaraComponent, ASensor::NiagaraHitColors, ASensor::NiagaraHitPoints, ASensor::NiagaraPointsFloat, and ASensor::NiagaraPointsInt.

Referenced by SetNiagaraRendering(), and VisualizeLidarParticles().

◆ VisualizeLidarParticles()

void ALidar::VisualizeLidarParticles ( std::vector< FPointData HitPoints)
private

Visualize Lidar particles with Niagara particle system

Definition at line 429 of file Lidar.cpp.

430{
431 TRACE_CPUPROFILER_EVENT_SCOPE(ALidar::VisualizeLidarParticles);
432
433 if (!LidarParameters.VisualizePointcloud || !NiagaraComponent || HitPoints.empty())
434 {
435 return;
436 }
437
438 if (GEngine && GEngine->GameViewport && GEngine->GameViewport->bDisableWorldRendering)
439 {
440 // If world rendering (Spectator camera) is disabled, no need to update the particle system.
441 return;
442 }
443
444 // Convert std::vector<FPointData> to
445 // TArray<FVector> and TArray<FLinearColor> for Niagara visualization.
446 int32 NumberOfHits = HitPoints.size();
447
448 // Create and allocate HitLocations TArray
449 TArray<FVector> HitLocations;
450 HitLocations.SetNumUninitialized(NumberOfHits);
451
452 // Create and set initial colors to green
453 TArray<FLinearColor> Colors;
454 Colors.Init(FLinearColor::Green, NumberOfHits);
455
456 // Pointer access for reduced overhead
457 FVector* HitLocationPtr = HitLocations.GetData();
458 FLinearColor* ColorsPtr = Colors.GetData();
459
460 const bool Semantic = LidarParameters.Semantic;
461
462 for (int32 i = 0; i < NumberOfHits; ++i)
463 {
464 const FPointData& point = HitPoints[i];
465
466 HitLocationPtr[i].X = point.X;
467 HitLocationPtr[i].Y = (point.Y * -1);
468 HitLocationPtr[i].Z = point.Z;
469
470 if (Semantic)
471 {
472 // Convert point.RGB (uint32) to FLinearColor
473 uint8 R = (point.RGB >> 16) & 0xFF;
474 uint8 G = (point.RGB >> 8) & 0xFF;
475 uint8 B = point.RGB & 0xFF;
476
477 ColorsPtr[i].R = R / 255.0f;
478 ColorsPtr[i].G = G / 255.0f;
479 ColorsPtr[i].B = B / 255.0f;
480 //ColorsPtr[i].A = 1.0f;
481 }
482 }
483 UpdateLidarParticles(NumberOfHits, HitLocations, Colors);
484}
float Z
Definition: PointData.h:19
float Y
Definition: PointData.h:17
uint32 RGB
Definition: PointData.h:21

References LidarParameters, NiagaraComponent, FPointData::RGB, FLidarParameters::Semantic, UpdateLidarParticles(), VisualizeLidarParticles(), FLidarParameters::VisualizePointcloud, FPointData::X, FPointData::Y, and FPointData::Z.

Referenced by ResetRecordedHits(), and VisualizeLidarParticles().

Friends And Related Function Documentation

◆ ALidarManager

friend class ALidarManager
friend

Definition at line 113 of file Lidar.h.

Member Data Documentation

◆ CachedNoneColor

uint32 ALidar::CachedNoneColor
private

Definition at line 268 of file Lidar.h.

Referenced by BeginPlay(), and GetSemanticData().

◆ CachedSnowflakeColor

uint32 ALidar::CachedSnowflakeColor
private

Definition at line 270 of file Lidar.h.

Referenced by BeginPlay(), and GetSemanticData().

◆ ClearContainers

bool ALidar::ClearContainers = true
private

Definition at line 256 of file Lidar.h.

Referenced by CanSendData(), ResetRecordedHits(), and SimulateRaycastLidar().

◆ CurrentHorizontalAngle

float ALidar::CurrentHorizontalAngle = 0.0f
private

Definition at line 260 of file Lidar.h.

Referenced by SimulateRaycastLidar().

◆ CurrentWeatherParameters

FWeatherParameters ALidar::CurrentWeatherParameters
private

Definition at line 236 of file Lidar.h.

Referenced by BeginPlay(), OnWeatherChanged(), ShootLaser(), and SimulateRaycastLidar().

◆ Generator

std::mt19937 ALidar::Generator
private

Definition at line 251 of file Lidar.h.

Referenced by AddDistanceNoise(), AddLateralNoise(), and Init().

◆ LaserAngles

std::vector<float> ALidar::LaserAngles
private

Definition at line 248 of file Lidar.h.

Referenced by ChangeParameters(), EndPlay(), and SimulateRaycastLidar().

◆ LidarParameters

FLidarParameters ALidar::LidarParameters
private

◆ LidarParametersChanged

bool ALidar::LidarParametersChanged = false
private

Definition at line 254 of file Lidar.h.

Referenced by ChangeLidarParameters(), and SimulateRaycastLidar().

◆ LidarSaves

int32 ALidar::LidarSaves = 0
private

Definition at line 266 of file Lidar.h.

Referenced by SaveDataToDisk().

◆ NeedToSavePointCloudWithoutNoise

bool ALidar::NeedToSavePointCloudWithoutNoise = false
private

Definition at line 253 of file Lidar.h.

Referenced by SaveDataToDisk(), ShootLaser(), and SimulateRaycastLidar().

◆ NiagaraComponent

UNiagaraComponent* ALidar::NiagaraComponent = nullptr
private

NiagaraComponent for visualizing Lidar particles. Particles are only visible on Spectator camera.

Definition at line 126 of file Lidar.h.

Referenced by BeginPlay(), EndPlay(), SetNiagaraRendering(), SetParticleLifeTime(), UpdateLidarParticles(), and VisualizeLidarParticles().

◆ PointCloudMessage

TSharedPtr<ROSMessages::sensor_msgs::PointCloud2> ALidar::PointCloudMessage
private

Definition at line 241 of file Lidar.h.

Referenced by EndPlay(), Init(), and SendDataToTopic().

◆ Points

std::vector<std::vector<FPointData> > ALidar::Points
private

Definition at line 245 of file Lidar.h.

Referenced by ChangeParameters(), EndPlay(), ResetRecordedHits(), SendData(), and SimulateRaycastLidar().

◆ PointsFlattened

std::vector<FPointData> ALidar::PointsFlattened
private

Definition at line 246 of file Lidar.h.

Referenced by EndPlay(), ResetRecordedHits(), SaveDataToDisk(), SendData(), and SimulateRaycastLidar().

◆ PreProcessedHitImpactPoints

std::vector<std::vector<FPointData> > ALidar::PreProcessedHitImpactPoints
private

Definition at line 243 of file Lidar.h.

Referenced by ChangeParameters(), EndPlay(), ResetRecordedHits(), SaveDataToDisk(), and ShootLaser().

◆ RandomDevice

std::random_device ALidar::RandomDevice
private

Definition at line 250 of file Lidar.h.

Referenced by Init().

◆ SemanticColors

TMap<FName, FColor> ALidar::SemanticColors
private

Definition at line 239 of file Lidar.h.

Referenced by BeginPlay(), and GetSemanticData().

◆ SendDataEveryFrame

bool ALidar::SendDataEveryFrame = false
private

Definition at line 255 of file Lidar.h.

Referenced by CanSendData(), ChangeParameters(), and ResetRecordedHits().

◆ SensorHzFrequency

float ALidar::SensorHzFrequency = 0.1f
private

Definition at line 261 of file Lidar.h.

Referenced by CanSendData(), and ChangeParameters().

◆ SensorHzTimer

float ALidar::SensorHzTimer = 0.0f
private

Definition at line 262 of file Lidar.h.

Referenced by CanSendData().

◆ SnowHeightOffset

float ALidar::SnowHeightOffset = 0.0f
private

Definition at line 264 of file Lidar.h.

Referenced by AddSnowTerrainAdjustment(), and OnWeatherChanged().

◆ TempLidarParameters

FLidarParameters ALidar::TempLidarParameters
private

Definition at line 234 of file Lidar.h.

Referenced by ChangeLidarParameters(), and SimulateRaycastLidar().

◆ TerrainTag

const FName ALidar::TerrainTag = "Terrain"
private

Definition at line 258 of file Lidar.h.

Referenced by AddSnowTerrainAdjustment().


The documentation for this class was generated from the following files: