Agrarsense
LidarManager.cpp
Go to the documentation of this file.
1// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
2//
3// This work is licensed under the terms of the MIT license.
4// For a copy, see <https://opensource.org/licenses/MIT>.
5
6#include "LidarManager.h"
7
13
14#include "ROSIntegration/Classes/ROSIntegrationGameInstance.h"
15#include "ROSIntegration/Classes/RI/Topic.h"
16
17#include "PhysicsEngine/PhysicsObjectExternalInterface.h"
18#include "Physics/Experimental/PhysScene_Chaos.h"
19#include "Physics/PhysicsInterfaceUtils.h"
20
21#include "Math/Vector.h"
22#include "Engine/CollisionProfile.h"
23#include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h"
24#include "Runtime/Core/Public/Async/ParallelFor.h"
25#include "Kismet/GameplayStatics.h"
26#include "Kismet/KismetMathLibrary.h"
27#include "Misc/CommandLine.h"
28
29#include <iterator>
30
31ALidarManager::ALidarManager(const FObjectInitializer& ObjectInitializer) : 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}
41
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}
71
72void ALidarManager::EndPlay(const EEndPlayReason::Type EndPlayReason)
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}
91
92void ALidarManager::TickParallel(float DeltaTime)
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}
111
112void ALidarManager::SimulateLidars(const float DeltaTime)
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 if (LidarCount == 1)
126 {
127 // If we only have one lidar sensor,
128 // we can skip ParallelFor and 2D to 1D std::vector conversion overhead completely
129 ALidar* lidar = RaycastLidars[0];
130 if (lidar)
131 {
132#ifdef ParallelLineTraceSingleByChannel_EXISTS
133 // ParallelLineTraceSingleByChannel_EXISTS Defined and implemented in AGRARSENSE Unreal Engine fork
134 // Lock physics scene while linetracing IF engine version is 5.5 or under.
135#if ENGINE_MAJOR_VERSION < 5 || (ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION <= 5)
136 auto PhysicsLock = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
137 {
138#endif
139#endif
140 FlattenedPoints = lidar->SimulateRaycastLidar(DeltaTime);
141
142#ifdef ParallelLineTraceSingleByChannel_EXISTS
143#if ENGINE_MAJOR_VERSION < 5 || (ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION <= 5)
144 }
145 PhysicsLock.Release();
146#endif
147#endif
148 }
149 }
150 else
151 {
152 // Else we have multiple Lidar sensors,
153 // Simulate all of them in parallel
154 std::vector<std::vector<FPointData>> points(LidarCount);
155
156#ifdef ParallelLineTraceSingleByChannel_EXISTS
157#if ENGINE_MAJOR_VERSION < 5 || (ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION <= 5)
158 // ParallelLineTraceSingleByChannel_EXISTS Defined and implemented in AGRARSENSE Unreal Engine fork
159 // Lock physics scene while linetracing IF engine version is 5.5 or under.
160 auto PhysicsLock = FPhysicsObjectExternalInterface::LockRead(GetWorld()->GetPhysicsScene());
161 {
162#endif
163#endif
164 ParallelFor(LidarCount, [&](int32 lidarIndex)
165 {
166 ALidar* lidar = RaycastLidars[lidarIndex];
167 if (lidar)
168 {
169 points[lidarIndex] = lidar->SimulateRaycastLidar(DeltaTime);
170 }
171 }, EParallelForFlags::Unbalanced);
172
173
174#ifdef ParallelLineTraceSingleByChannel_EXISTS
175#if ENGINE_MAJOR_VERSION < 5 || (ENGINE_MAJOR_VERSION == 5 && ENGINE_MINOR_VERSION <= 5)
176 }
177 PhysicsLock.Release();
178#endif
179#endif
180
182 {
183 int32 TotalSize = 0;
184 for (const std::vector<FPointData>& vec : points)
185 {
186 TotalSize += vec.size();
187 }
188
189 if (TotalSize != 0)
190 {
191 FlattenedPoints.reserve(TotalSize);
192 for (std::vector<FPointData>& vec : points)
193 {
194 FlattenedPoints.insert(FlattenedPoints.end(), std::make_move_iterator(vec.begin()), std::make_move_iterator(vec.end()));
195 }
196 }
197 }
198 }
199
200 if (!FlattenedPoints.empty())
201 {
203 {
204 SaveCombinedPointcloud(FlattenedPoints);
205 }
206
207 SendCombinedPointcloud(FlattenedPoints);
208 }
209}
210
211void ALidarManager::SetSimulateLidars(bool bSimulateLidars)
212{
213 CanSimulateLidars = bSimulateLidars;
214}
215
216void ALidarManager::SetMeasurePerformance(bool bMeasurePerformance)
217{
218 MeasurePerformance = bMeasurePerformance;
219}
220
221void ALidarManager::SetSaveCombinedCloudToDisk(bool bSaveCombinedCloud)
222{
223 SaveCombinedCloudToDisk = bSaveCombinedCloud;
224}
225
226void ALidarManager::SetSendCombinedCloud(bool bSaveCombinedCloud)
227{
228 SendCombinedCloudToROS = bSaveCombinedCloud;
230 {
231 InitTopic();
232 }
233
234#if WITH_EDITOR
235 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Saving combined Lidar pointcloud to disk: %s"), (SendCombinedCloudToROS ? TEXT("true") : TEXT("false")));
236#endif
237}
238
240{
241 if (!lidarPtr)
242 {
243 return;
244 }
245
246 RaycastLidars.push_back(lidarPtr);
247
248#if WITH_EDITOR
249 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Added Lidar sensor."));
250#endif
251
252 if (!TickAdded)
253 {
254 InitTopic();
255
256 TickEntry = ATickManager::AddTick(this, BindTick(this, &ALidarManager::TickParallel), ETickType::FullFrameTaskParallel);
257 TickAdded = true;
258 }
259
260 // Broadcast new lidar has been spawned, broadcast Lidar actor
261 OnLidarSpawned.Broadcast(lidarPtr);
262}
263
265{
266 if (!lidarPtr)
267 {
268 return;
269 }
270
271 auto it = std::find(RaycastLidars.begin(), RaycastLidars.end(), lidarPtr);
272 if (it != RaycastLidars.end())
273 {
274 RaycastLidars.erase(it);
275
276#if WITH_EDITOR
277 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Removed Lidar sensor"));
278#endif
279 }
280
281 // Disable tick if there's no Lidar sensors.
282 if (RaycastLidars.size() == 0)
283 {
285 if (TickAdded)
286 {
288 TickAdded = false;
289 DestroyTopic();
290 }
291 }
292
293 // Broadcast lidar sensor has been destroyed event and broadcast current lidar count
294 int32 currentLidarCount = RaycastLidars.size();
295 OnLidarDestroyed.Broadcast(currentLidarCount);
296}
297
299{
300 switch (RosState)
301 {
303 InitTopic();
304 break;
305
307 DestroyTopic();
308 break;
309 }
310}
311
313{
314 if (CombinedLidarTopic.IsValid() || !SendCombinedCloudToROS || !RosInstance->IsROSConnected())
315 {
316 return;
317 }
318
319 CombinedLidarTopic = NewObject<UTopic>(UTopic::StaticClass());
320 CombinedLidarTopic->Init(RosInstance->ROSIntegrationCore, FString::Printf(TEXT("/agrarsense/out/sensors/lidars_combined")), TEXT("sensor_msgs/PointCloud2"));
321 CombinedLidarTopic->Advertise();
322
323#if WITH_EDITOR
324 UE_LOG(LogTemp, Warning, TEXT("LidarManager.cpp: Created combined Lidar ROS topic. Topic name: /agrarsense/sensors/lidars_combined"));
325#endif
326}
327
329{
330 if (CombinedLidarTopic.IsValid())
331 {
332 CombinedLidarTopic->Unadvertise();
333 CombinedLidarTopic->Unsubscribe();
334 CombinedLidarTopic->MarkAsDisconnected();
335 CombinedLidarTopic->ConditionalBeginDestroy();
336 CombinedLidarTopic.Reset();
337 }
338}
339
340void ALidarManager::SaveCombinedPointcloud(const std::vector<FPointData>& points)
341{
343 {
344 return;
345 }
346
347 FString Filename = FString::Printf(TEXT("%sCombinedPointcloud_%d.ply"), *FileSavePath, LidarSaves);
348 ++LidarSaves;
349
351}
352
353void ALidarManager::SendCombinedPointcloud(const std::vector<FPointData>& points)
354{
355 TRACE_CPUPROFILER_EVENT_SCOPE(ALidarManager::SendCombinedPointcloud);
356
357 UTopic* Topic = CombinedLidarTopic.Get();
358
360 || !RosInstance
361 || !RosInstance->IsROSConnected()
362 || !Topic
363 || !PointCloudMessage.IsValid())
364 {
365 return;
366 }
367
368 const size_t PointsSize = points.size();
369 const size_t DataSize = PointsSize * sizeof(FPointData);
370
371 PointCloudMessage->width = static_cast<uint32>(PointsSize);
372 PointCloudMessage->row_step = static_cast<uint32>(DataSize);
373 PointCloudMessage->data_ptr = reinterpret_cast<uint8*>(const_cast<FPointData*>(points.data()));
374 Topic->Publish(PointCloudMessage);
375}
EROSState
Definition: ROSState.h:16
static auto BindTick(ObjectType *Object, FunctionType Function)
Definition: TickManager.h:162
bool CanSimulateLidars
Definition: LidarManager.h:213
void SendCombinedPointcloud(const std::vector< FPointData > &points)
FString FileSavePath
Definition: LidarManager.h:215
FLidarDelegate_LidarSpawned OnLidarSpawned
Definition: LidarManager.h:131
UROSIntegrationGameInstance * RosInstance
Definition: LidarManager.h:204
void SaveCombinedPointcloud(const std::vector< FPointData > &points)
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > PointCloudMessage
Definition: LidarManager.h:209
virtual void BeginPlay() override
TWeakObjectPtr< UTopic > CombinedLidarTopic
Definition: LidarManager.h:207
void SetSaveCombinedCloudToDisk(bool bSaveCombinedCloud)
void AddRaycastLidar(ALidar *lidarPtr)
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
bool SaveCombinedCloudToDisk
Definition: LidarManager.h:144
FTickEntry TickEntry
Definition: LidarManager.h:160
void ROSBridgeStateChanged(EROSState RosState)
void SetSendCombinedCloud(bool bSaveCombinedCloud)
void SetMeasurePerformance(bool bMeasurePerformance)
void SetSimulateLidars(bool bSimulateLidars)
float TimeInMilliseconds
Definition: LidarManager.h:150
void RemoveRaycastLidar(ALidar *lidarPtr)
SimpleTimer * TotalAverageTimer
Definition: LidarManager.h:211
void SimulateLidars(const float DeltaTime)
ALidarManager(const FObjectInitializer &ObjectInitializer)
bool SendCombinedCloudToROS
Definition: LidarManager.h:141
std::vector< ALidar * > RaycastLidars
Definition: LidarManager.h:201
FLidarDelegate_LidarDestroyed OnLidarDestroyed
Definition: LidarManager.h:138
void TickParallel(float DeltaTime)
bool MeasurePerformance
Definition: LidarManager.h:147
Definition: Lidar.h:35
std::vector< FPointData > SimulateRaycastLidar(const float DeltaTime)
Definition: Lidar.cpp:249
static void RemoveTick(FTickEntry TickEntry)
Definition: TickManager.cpp:80
static FTickEntry AddTick(UObject *Object, std::function< void(float)> Function, ETickType Type)
Definition: TickManager.cpp:51
double PrintAverageTime(FString prefix="Average execution time ", FString suffix=" milliseconds", bool printToScreen=false)
Definition: SimpleTimer.h:123
void TakeTimeStamp()
Definition: SimpleTimer.h:76
void ResetAverageTime()
Definition: SimpleTimer.h:141
static FString GetDataFolder()
static void SaveVectorArrayAsPlyAsync(FString FullFileName, const std::vector< FPointData > points)