Agrarsense
Public Member Functions | Public Attributes | Private Member Functions | Private Attributes | List of all members
ALidarManager Class Reference

#include <LidarManager.h>

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

Public Member Functions

 ALidarManager (const FObjectInitializer &ObjectInitializer)
 
void SetSimulateLidars (bool bSimulateLidars)
 
void SetMeasurePerformance (bool bMeasurePerformance)
 
void SetSaveCombinedCloudToDisk (bool bSaveCombinedCloud)
 
void SetSendCombinedCloud (bool bSaveCombinedCloud)
 
void AddRaycastLidar (ALidar *lidarPtr)
 
void RemoveRaycastLidar (ALidar *lidarPtr)
 
int32 GetLidarCount () const
 
TArray< ALidar * > GetAllLidars () const
 
std::vector< ALidar * > GetAllLidarsVector () const
 
 UPROPERTY (BlueprintReadOnly)
 

Public Attributes

FLidarDelegate_LidarSpawned OnLidarSpawned
 
FLidarDelegate_LidarDestroyed OnLidarDestroyed
 
bool SendCombinedCloudToROS = true
 
bool SaveCombinedCloudToDisk = false
 
bool MeasurePerformance = false
 
float TimeInMilliseconds
 

Private Member Functions

virtual void BeginPlay () override
 
virtual void EndPlay (const EEndPlayReason::Type EndPlayReason) override
 
void TickParallel (float DeltaTime)
 
void SimulateLidars (const float DeltaTime)
 
void SendCombinedPointcloud (const std::vector< FPointData > &points)
 
void SaveCombinedPointcloud (const std::vector< FPointData > &points)
 
void ROSBridgeStateChanged (EROSState RosState)
 
void InitTopic ()
 
void DestroyTopic ()
 

Private Attributes

FTickEntry TickEntry
 
bool TickAdded = false
 
std::vector< ALidar * > RaycastLidars
 
UROSIntegrationGameInstance * RosInstance = nullptr
 
TWeakObjectPtr< UTopic > CombinedLidarTopic
 
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > PointCloudMessage
 
SimpleTimerTotalAverageTimer = nullptr
 
bool CanSimulateLidars = true
 
FString FileSavePath
 
int32 LidarSaves = 0
 

Detailed Description

This class handles simulating all Lidar sensors in parallel.

Definition at line 40 of file LidarManager.h.

Constructor & Destructor Documentation

◆ ALidarManager()

ALidarManager::ALidarManager ( const FObjectInitializer &  ObjectInitializer)

Definition at line 31 of file LidarManager.cpp.

31 : Super(ObjectInitializer)
32{
33 // TickManager handles this Actor ticking
34 PrimaryActorTick.bCanEverTick = false;
35
36 if (FParse::Param(FCommandLine::Get(), TEXT("-save-combined-pointcloud")))
37 {
39 }
40}
bool SaveCombinedCloudToDisk
Definition: LidarManager.h:144

References SaveCombinedCloudToDisk.

Member Function Documentation

◆ AddRaycastLidar()

void ALidarManager::AddRaycastLidar ( ALidar lidarPtr)

Add Lidar sensor to this manager. This should only be called from Lidar.cpp

Parameters
lidarPtrLidar class pointer

Definition at line 212 of file LidarManager.cpp.

213{
214 if (!lidarPtr)
215 {
216 return;
217 }
218
219 RaycastLidars.push_back(lidarPtr);
220
221#if WITH_EDITOR
222 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Added Lidar sensor."));
223#endif
224
225 if (!TickAdded)
226 {
227 InitTopic();
228
229 TickEntry = ATickManager::AddTick(this, BindTick(this, &ALidarManager::TickParallel), ETickType::FullFrameTaskParallel);
230 TickAdded = true;
231 }
232
233 // Broadcast new lidar has been spawned, broadcast Lidar actor
234 OnLidarSpawned.Broadcast(lidarPtr);
235}
static auto BindTick(ObjectType *Object, FunctionType Function)
Definition: TickManager.h:162
FLidarDelegate_LidarSpawned OnLidarSpawned
Definition: LidarManager.h:131
FTickEntry TickEntry
Definition: LidarManager.h:160
std::vector< ALidar * > RaycastLidars
Definition: LidarManager.h:201
void TickParallel(float DeltaTime)
static FTickEntry AddTick(UObject *Object, std::function< void(float)> Function, ETickType Type)
Definition: TickManager.cpp:51

References ATickManager::AddTick(), BindTick(), InitTopic(), OnLidarSpawned, RaycastLidars, TickAdded, TickEntry, and TickParallel().

Referenced by ALidar::BeginPlay().

◆ BeginPlay()

void ALidarManager::BeginPlay ( )
overrideprivatevirtual

Definition at line 42 of file LidarManager.cpp.

43{
44 Super::BeginPlay();
45
46 // There should only be one instance of this actor which is spawned in AgrarsenseGameModebase.cpp
47 UWorld* World = GetWorld();
48 TArray<AActor*> FoundActors;
49 UGameplayStatics::GetAllActorsOfClass(World, ALidarManager::StaticClass(), FoundActors);
50 if (FoundActors.Num() != 1)
51 {
52#ifdef WITH_EDITOR
53 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: LidarManager already exists in the level. Destroying this one."));
54#endif
55 this->Destroy();
56 return;
57 }
58
59 RosInstance = Cast<UROSIntegrationGameInstance>(GetGameInstance());
60
62
63 FString DataLocation = UAgrarsensePaths::GetDataFolder();
64 FString UnixTimeStamp = FString::FromInt(FDateTime::Now().ToUnixTimestamp());
65 FileSavePath = DataLocation + "LidarCombined_" + UnixTimeStamp + "/";
66
67 // Initialize the PointCloudMessage
68 PointCloudMessage = MakeShared<ROSMessages::sensor_msgs::PointCloud2>();
69 PointCloudMessage->CreateDefaultPointCloud2Message();
70}
FString FileSavePath
Definition: LidarManager.h:215
UROSIntegrationGameInstance * RosInstance
Definition: LidarManager.h:204
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > PointCloudMessage
Definition: LidarManager.h:209
SimpleTimer * TotalAverageTimer
Definition: LidarManager.h:211
static FString GetDataFolder()

References Destroy, FileSavePath, UAgrarsensePaths::GetDataFolder(), PointCloudMessage, RosInstance, and TotalAverageTimer.

◆ DestroyTopic()

void ALidarManager::DestroyTopic ( )
private

Destroy ROS topic

Definition at line 301 of file LidarManager.cpp.

302{
303 if (CombinedLidarTopic.IsValid())
304 {
305 CombinedLidarTopic->Unadvertise();
306 CombinedLidarTopic->Unsubscribe();
307 CombinedLidarTopic->MarkAsDisconnected();
308 CombinedLidarTopic->ConditionalBeginDestroy();
309 CombinedLidarTopic.Reset();
310 }
311}
TWeakObjectPtr< UTopic > CombinedLidarTopic
Definition: LidarManager.h:207

References CombinedLidarTopic.

Referenced by EndPlay(), RemoveRaycastLidar(), and ROSBridgeStateChanged().

◆ EndPlay()

void ALidarManager::EndPlay ( const EEndPlayReason::Type  EndPlayReason)
overrideprivatevirtual

Definition at line 72 of file LidarManager.cpp.

73{
74 Super::EndPlay(EndPlayReason);
75
77
78 PointCloudMessage.Reset();
79
80 RaycastLidars.clear();
81
82 RosInstance = nullptr;
83
84 delete(TotalAverageTimer);
85
86 if (TickAdded)
87 {
89 }
90}
static void RemoveTick(FTickEntry TickEntry)
Definition: TickManager.cpp:80

References DestroyTopic(), PointCloudMessage, RaycastLidars, ATickManager::RemoveTick(), RosInstance, TickAdded, TickEntry, and TotalAverageTimer.

◆ GetAllLidars()

TArray< ALidar * > ALidarManager::GetAllLidars ( ) const
inline

Get all Lidar sensors added to this Lidar Mananger

Returns
Tarray of ALidar pointers.

Definition at line 103 of file LidarManager.h.

104 {
105 // Create TArray<ALidar*> since LidarManager uses std::vector
106 // to keep track of Lidar sensors and Blueprints only support TArrays
107 TArray<ALidar*> Lidars;
108
109 for (int32 i = 0; i < RaycastLidars.size(); i++)
110 {
111 ALidar* LidarPtr = RaycastLidars[i];
112 if (LidarPtr)
113 {
114 Lidars.Add(LidarPtr);
115 }
116 }
117
118 return Lidars;
119 }
Definition: Lidar.h:35

◆ GetAllLidarsVector()

std::vector< ALidar * > ALidarManager::GetAllLidarsVector ( ) const
inline

Definition at line 121 of file LidarManager.h.

122 {
123 return RaycastLidars;
124 }

◆ GetLidarCount()

int32 ALidarManager::GetLidarCount ( ) const
inline

Get current Lidar count. Callable from blueprints.

Returns
int32 lidar count

Definition at line 93 of file LidarManager.h.

94 {
95 return RaycastLidars.size();
96 }

◆ InitTopic()

void ALidarManager::InitTopic ( )
private

Initialize ROS topic

Definition at line 285 of file LidarManager.cpp.

286{
287 if (CombinedLidarTopic.IsValid() || !SendCombinedCloudToROS || !RosInstance->IsROSConnected())
288 {
289 return;
290 }
291
292 CombinedLidarTopic = NewObject<UTopic>(UTopic::StaticClass());
293 CombinedLidarTopic->Init(RosInstance->ROSIntegrationCore, FString::Printf(TEXT("/agrarsense/out/sensors/lidars_combined")), TEXT("sensor_msgs/PointCloud2"));
294 CombinedLidarTopic->Advertise();
295
296#if WITH_EDITOR
297 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Created combined Lidar ROS topic. Topic name: /agrarsense/sensors/lidars_combined"));
298#endif
299}
bool SendCombinedCloudToROS
Definition: LidarManager.h:141

References CombinedLidarTopic, RosInstance, and SendCombinedCloudToROS.

Referenced by AddRaycastLidar(), ROSBridgeStateChanged(), and SetSendCombinedCloud().

◆ RemoveRaycastLidar()

void ALidarManager::RemoveRaycastLidar ( ALidar lidarPtr)

Remove Lidar sensor from this manager. This should only be called from Lidar.cpp

Parameters
lidarPtrLidar class pointer

Definition at line 237 of file LidarManager.cpp.

238{
239 if (!lidarPtr)
240 {
241 return;
242 }
243
244 auto it = std::find(RaycastLidars.begin(), RaycastLidars.end(), lidarPtr);
245 if (it != RaycastLidars.end())
246 {
247 RaycastLidars.erase(it);
248
249#if WITH_EDITOR
250 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Removed Lidar sensor"));
251#endif
252 }
253
254 // Disable tick if there's no Lidar sensors.
255 if (RaycastLidars.size() == 0)
256 {
258 if (TickAdded)
259 {
261 TickAdded = false;
262 DestroyTopic();
263 }
264 }
265
266 // Broadcast lidar sensor has been destroyed event and broadcast current lidar count
267 int32 currentLidarCount = RaycastLidars.size();
268 OnLidarDestroyed.Broadcast(currentLidarCount);
269}
FLidarDelegate_LidarDestroyed OnLidarDestroyed
Definition: LidarManager.h:138
void ResetAverageTime()
Definition: SimpleTimer.h:141

References DestroyTopic(), OnLidarDestroyed, RaycastLidars, ATickManager::RemoveTick(), SimpleTimer::ResetAverageTime(), TickAdded, TickEntry, and TotalAverageTimer.

Referenced by ALidar::EndPlay().

◆ ROSBridgeStateChanged()

void ALidarManager::ROSBridgeStateChanged ( EROSState  RosState)
private

Callback when ROS Bridge state changed. must be marked as UFUNCTION, otherwise binding to event will not work.

Definition at line 271 of file LidarManager.cpp.

272{
273 switch (RosState)
274 {
276 InitTopic();
277 break;
278
280 DestroyTopic();
281 break;
282 }
283}

References Connected, DestroyTopic(), Disconnected, and InitTopic().

◆ SaveCombinedPointcloud()

void ALidarManager::SaveCombinedPointcloud ( const std::vector< FPointData > &  points)
private

Save combined cloud as ply to disk

Definition at line 313 of file LidarManager.cpp.

314{
316 {
317 return;
318 }
319
320 FString Filename = FString::Printf(TEXT("%sCombinedPointcloud_%d.ply"), *FileSavePath, LidarSaves);
321 ++LidarSaves;
322
324}
static void SaveVectorArrayAsPlyAsync(FString FullFileName, const std::vector< FPointData > points)

References FileSavePath, LidarSaves, SaveCombinedCloudToDisk, and UPointcloudUtilities::SaveVectorArrayAsPlyAsync().

Referenced by SimulateLidars().

◆ SendCombinedPointcloud()

void ALidarManager::SendCombinedPointcloud ( const std::vector< FPointData > &  points)
private

Send combined pointcloud to ROS topic

Definition at line 326 of file LidarManager.cpp.

327{
328 TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SendCombinedPointcloud);
329
330 UTopic* Topic = CombinedLidarTopic.Get();
331
333 || !RosInstance
334 || !RosInstance->IsROSConnected()
335 || !Topic
336 || !PointCloudMessage.IsValid())
337 {
338 return;
339 }
340
341 const size_t PointsSize = points.size();
342 const size_t DataSize = PointsSize * sizeof(FPointData);
343
344 PointCloudMessage->width = static_cast<uint32>(PointsSize);
345 PointCloudMessage->row_step = static_cast<uint32>(DataSize);
346 PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
347 Topic->Publish(PointCloudMessage);
348}
void SendCombinedPointcloud(const std::vector< FPointData > &points)

References CombinedLidarTopic, PointCloudMessage, RosInstance, SendCombinedCloudToROS, and SendCombinedPointcloud().

Referenced by SendCombinedPointcloud(), and SimulateLidars().

◆ SetMeasurePerformance()

void ALidarManager::SetMeasurePerformance ( bool  bMeasurePerformance)

Set boolean whether to measure Lidar performance (logs milliseconds to console)

Parameters
bMeasurePerformanceboolean to toggle performance measurement

Definition at line 189 of file LidarManager.cpp.

190{
191 MeasurePerformance = bMeasurePerformance;
192}
bool MeasurePerformance
Definition: LidarManager.h:147

References MeasurePerformance.

◆ SetSaveCombinedCloudToDisk()

void ALidarManager::SetSaveCombinedCloudToDisk ( bool  bSaveCombinedCloud)

Set boolean whether to save combined cloud to disk

Parameters
bSaveCombinedCloudboolean to set saving to disk

Definition at line 194 of file LidarManager.cpp.

195{
196 SaveCombinedCloudToDisk = bSaveCombinedCloud;
197}

References SaveCombinedCloudToDisk.

Referenced by UROSCommands::HandleSetSaveCombinedPointcloudToDisk().

◆ SetSendCombinedCloud()

void ALidarManager::SetSendCombinedCloud ( bool  bSaveCombinedCloud)

Set boolean whether to send combined cloud to ROS

Parameters
bSaveCombinedCloudboolean to set sending data to ROS

Definition at line 199 of file LidarManager.cpp.

200{
201 SendCombinedCloudToROS = bSaveCombinedCloud;
203 {
204 InitTopic();
205 }
206
207#if WITH_EDITOR
208 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Saving combined Lidar pointcloud to disk: %s"), (SendCombinedCloudToROS ? TEXT("true") : TEXT("false")));
209#endif
210}

References InitTopic(), and SendCombinedCloudToROS.

◆ SetSimulateLidars()

void ALidarManager::SetSimulateLidars ( bool  bSimulateLidars)

Set boolean whether Lidar sensors be simulated

Parameters
bSimulateLidarsboolean to set simulating Lidars on/off

Definition at line 184 of file LidarManager.cpp.

185{
186 CanSimulateLidars = bSimulateLidars;
187}
bool CanSimulateLidars
Definition: LidarManager.h:213

References CanSimulateLidars.

◆ SimulateLidars()

void ALidarManager::SimulateLidars ( const float  DeltaTime)
private

Simulate all Lidar sensors

Parameters
DeltaTimecurrent DeltaTime

Definition at line 112 of file LidarManager.cpp.

113{
114 TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SimulateLidars);
115
116 const int32 LidarCount = RaycastLidars.size();
117
118 if (LidarCount == 0 || !CanSimulateLidars)
119 {
120 return;
121 }
122
123 std::vector<FPointData> FlattenedPoints;
124
125 FPhysicsCommand::ExecuteRead(GetWorld()->GetPhysicsScene(), [&]
126 {
127 if (LidarCount == 1)
128 {
129 // If we only have one lidar sensor,
130 // we can skip ParallelFor and 2D to 1D std::vector conversion
131 ALidar* lidar = RaycastLidars[0];
132 if (lidar)
133 {
134 FlattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
135 }
136 }
137 else
138 {
139 // Else we have multiple Lidar sensors,
140 // Simulate all of them in parallel
141 std::vector<std::vector<FPointData>> points(LidarCount);
142
143 ParallelFor(LidarCount, [&](int32 lidarIndex)
144 {
145 ALidar* lidar = RaycastLidars[lidarIndex];
146 if (lidar)
147 {
148 points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
149 }
150 }, EParallelForFlags::Unbalanced);
151
153 {
154 int32 TotalSize = 0;
155 for (const std::vector<FPointData>& vec : points)
156 {
157 TotalSize += vec.size();
158 }
159
160 if (TotalSize != 0)
161 {
162 FlattenedPoints.reserve(TotalSize);
163 for (std::vector<FPointData>& vec : points)
164 {
165 FlattenedPoints.insert(FlattenedPoints.end(), std::make_move_iterator(vec.begin()), std::make_move_iterator(vec.end()));
166 }
167 }
168 }
169 }
170
171 });
172
173 if (!FlattenedPoints.empty())
174 {
176 {
177 SaveCombinedPointcloud(FlattenedPoints);
178 }
179
180 SendCombinedPointcloud(FlattenedPoints);
181 }
182}
void SaveCombinedPointcloud(const std::vector< FPointData > &points)
void SimulateLidars(const float DeltaTime)
std::vector< FPointData > SimulateRaycastLidar(const float DeltaTime)
Definition: Lidar.cpp:249

References CanSimulateLidars, RaycastLidars, SaveCombinedCloudToDisk, SaveCombinedPointcloud(), SendCombinedCloudToROS, SendCombinedPointcloud(), SimulateLidars(), and ALidar::SimulateRaycastLidar().

Referenced by SimulateLidars(), and TickParallel().

◆ TickParallel()

void ALidarManager::TickParallel ( float  DeltaTime)
private

Definition at line 92 of file LidarManager.cpp.

93{
94#ifdef WITH_EDITOR
96 {
98 }
99#endif
100
101 SimulateLidars(DeltaTime);
102
103#ifdef WITH_EDITOR
105 {
106 double ms = TotalAverageTimer->PrintAverageTime();
107 TimeInMilliseconds = FMath::RoundToFloat(ms * 100.0f) / 100.0f;
108 }
109#endif
110}
float TimeInMilliseconds
Definition: LidarManager.h:150
double PrintAverageTime(FString prefix="Average execution time ", FString suffix=" milliseconds", bool printToScreen=false)
Definition: SimpleTimer.h:123
void TakeTimeStamp()
Definition: SimpleTimer.h:76

References MeasurePerformance, SimpleTimer::PrintAverageTime(), SimulateLidars(), SimpleTimer::TakeTimeStamp(), TimeInMilliseconds, and TotalAverageTimer.

Referenced by AddRaycastLidar().

◆ UPROPERTY()

ALidarManager::UPROPERTY ( BlueprintReadOnly  )

Member Data Documentation

◆ CanSimulateLidars

bool ALidarManager::CanSimulateLidars = true
private

Definition at line 213 of file LidarManager.h.

Referenced by SetSimulateLidars(), and SimulateLidars().

◆ CombinedLidarTopic

TWeakObjectPtr<UTopic> ALidarManager::CombinedLidarTopic
private

Definition at line 207 of file LidarManager.h.

Referenced by DestroyTopic(), InitTopic(), and SendCombinedPointcloud().

◆ FileSavePath

FString ALidarManager::FileSavePath
private

Definition at line 215 of file LidarManager.h.

Referenced by BeginPlay(), and SaveCombinedPointcloud().

◆ LidarSaves

int32 ALidarManager::LidarSaves = 0
private

Definition at line 217 of file LidarManager.h.

Referenced by SaveCombinedPointcloud().

◆ MeasurePerformance

bool ALidarManager::MeasurePerformance = false

Definition at line 147 of file LidarManager.h.

Referenced by SetMeasurePerformance(), and TickParallel().

◆ OnLidarDestroyed

FLidarDelegate_LidarDestroyed ALidarManager::OnLidarDestroyed

On Lidar sensor destroyed event. BlueprintAssignable.

Returns
int32 Lidar sensor count

Definition at line 138 of file LidarManager.h.

Referenced by RemoveRaycastLidar().

◆ OnLidarSpawned

FLidarDelegate_LidarSpawned ALidarManager::OnLidarSpawned

On Lidar sensor spawned event. BlueprintAssignable.

Returns
ALidar Lidar class pointer

Definition at line 131 of file LidarManager.h.

Referenced by AddRaycastLidar().

◆ PointCloudMessage

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

Definition at line 209 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), and SendCombinedPointcloud().

◆ RaycastLidars

std::vector<ALidar*> ALidarManager::RaycastLidars
private

Container that holds references to Lidar sensors

Definition at line 201 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), RemoveRaycastLidar(), and SimulateLidars().

◆ RosInstance

UROSIntegrationGameInstance* ALidarManager::RosInstance = nullptr
private

Definition at line 204 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), InitTopic(), and SendCombinedPointcloud().

◆ SaveCombinedCloudToDisk

bool ALidarManager::SaveCombinedCloudToDisk = false

◆ SendCombinedCloudToROS

bool ALidarManager::SendCombinedCloudToROS = true

◆ TickAdded

bool ALidarManager::TickAdded = false
private

Definition at line 162 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().

◆ TickEntry

FTickEntry ALidarManager::TickEntry
private

Definition at line 160 of file LidarManager.h.

Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().

◆ TimeInMilliseconds

float ALidarManager::TimeInMilliseconds

Definition at line 150 of file LidarManager.h.

Referenced by TickParallel().

◆ TotalAverageTimer

SimpleTimer* ALidarManager::TotalAverageTimer = nullptr
private

Definition at line 211 of file LidarManager.h.

Referenced by BeginPlay(), EndPlay(), RemoveRaycastLidar(), and TickParallel().


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