Agrarsense
|
#include <LidarManager.h>
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) |
int | 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 | SimulateRaycastLidars (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 |
SimpleTimer * | TotalAverageTimer = nullptr |
bool | SimulateLidars = true |
FString | FileSavePath |
int | LidarSaves = 0 |
TSharedPtr< ROSMessages::sensor_msgs::PointCloud2 > | PointCloudMessage |
This class handles simulating all Lidar sensors in parallel.
Definition at line 40 of file LidarManager.h.
ALidarManager::ALidarManager | ( | const FObjectInitializer & | ObjectInitializer | ) |
Definition at line 31 of file LidarManager.cpp.
References SaveCombinedCloudToDisk.
void ALidarManager::AddRaycastLidar | ( | ALidar * | lidarPtr | ) |
Add Lidar sensor to this manager. This should only be called from Lidar.cpp
lidarPtr | Lidar class pointer |
Definition at line 255 of file LidarManager.cpp.
References ATickManager::AddTick(), BindTick(), InitTopic(), OnLidarSpawned, RaycastLidars, TickAdded, TickEntry, and TickParallel().
Referenced by ALidar::BeginPlay().
|
overrideprivatevirtual |
Definition at line 42 of file LidarManager.cpp.
References Destroy, FileSavePath, UAgrarsensePaths::GetDataFolder(), PointCloudMessage, RosInstance, and TotalAverageTimer.
|
private |
Destroy ROS topic
Definition at line 344 of file LidarManager.cpp.
References CombinedLidarTopic.
Referenced by EndPlay(), RemoveRaycastLidar(), and ROSBridgeStateChanged().
|
overrideprivatevirtual |
Definition at line 97 of file LidarManager.cpp.
References DestroyTopic(), PointCloudMessage, RaycastLidars, ATickManager::RemoveTick(), RosInstance, TickAdded, TickEntry, and TotalAverageTimer.
|
inline |
Get all Lidar sensors added to this Lidar Mananger
Definition at line 103 of file LidarManager.h.
|
inline |
Definition at line 121 of file LidarManager.h.
|
inline |
Get current Lidar count. Callable from blueprints.
Definition at line 93 of file LidarManager.h.
|
private |
Initialize ROS topic
Definition at line 328 of file LidarManager.cpp.
References CombinedLidarTopic, RosInstance, and SendCombinedCloudToROS.
Referenced by AddRaycastLidar(), ROSBridgeStateChanged(), and SetSendCombinedCloud().
void ALidarManager::RemoveRaycastLidar | ( | ALidar * | lidarPtr | ) |
Remove Lidar sensor from this manager. This should only be called from Lidar.cpp
lidarPtr | Lidar class pointer |
Definition at line 280 of file LidarManager.cpp.
References DestroyTopic(), OnLidarDestroyed, RaycastLidars, ATickManager::RemoveTick(), SimpleTimer::ResetAverageTime(), TickAdded, TickEntry, and TotalAverageTimer.
Referenced by ALidar::EndPlay().
|
private |
Callback when ROS Bridge state changed. must be marked as UFUNCTION, otherwise binding to event will not work.
Definition at line 314 of file LidarManager.cpp.
References Connected, DestroyTopic(), Disconnected, and InitTopic().
|
private |
Save combined cloud as ply to disk
Definition at line 356 of file LidarManager.cpp.
References FileSavePath, LidarSaves, SaveCombinedCloudToDisk, and UPointcloudUtilities::SaveVectorArrayAsPlyAsync().
Referenced by SimulateRaycastLidars().
|
private |
Send combined pointcloud to ROS topic
Definition at line 369 of file LidarManager.cpp.
References CombinedLidarTopic, PointCloudMessage, RosInstance, SendCombinedCloudToROS, and SendCombinedPointcloud().
Referenced by SendCombinedPointcloud(), and SimulateRaycastLidars().
void ALidarManager::SetMeasurePerformance | ( | bool | bMeasurePerformance | ) |
Set boolean whether to measure Lidar performance (logs milliseconds to console)
bMeasurePerformance | boolean to toggle performance measurement |
Definition at line 232 of file LidarManager.cpp.
References MeasurePerformance.
void ALidarManager::SetSaveCombinedCloudToDisk | ( | bool | bSaveCombinedCloud | ) |
Set boolean whether to save combined cloud to disk
bSaveCombinedCloud | boolean to set saving to disk |
Definition at line 237 of file LidarManager.cpp.
References SaveCombinedCloudToDisk.
Referenced by UROSCommands::HandleSetSaveCombinedPointcloudToDisk().
void ALidarManager::SetSendCombinedCloud | ( | bool | bSaveCombinedCloud | ) |
Set boolean whether to send combined cloud to ROS
bSaveCombinedCloud | boolean to set sending data to ROS |
Definition at line 242 of file LidarManager.cpp.
References InitTopic(), and SendCombinedCloudToROS.
void ALidarManager::SetSimulateLidars | ( | bool | bSimulateLidars | ) |
Set boolean whether Lidar sensors be simulated
bSimulateLidars | boolean to set simulating Lidars on/off |
Definition at line 227 of file LidarManager.cpp.
References SimulateLidars.
|
private |
Simulate all Lidar sensors
DeltaTime | current DeltaTime |
Definition at line 137 of file LidarManager.cpp.
References RaycastLidars, SaveCombinedCloudToDisk, SaveCombinedPointcloud(), SendCombinedCloudToROS, SendCombinedPointcloud(), SimulateLidars, ALidar::SimulateRaycastLidar(), and SimulateRaycastLidars().
Referenced by SimulateRaycastLidars(), and TickParallel().
|
private |
Definition at line 117 of file LidarManager.cpp.
References MeasurePerformance, SimpleTimer::PrintAverageTime(), SimulateRaycastLidars(), SimpleTimer::TakeTimeStamp(), TimeInMilliseconds, and TotalAverageTimer.
Referenced by AddRaycastLidar().
ALidarManager::UPROPERTY | ( | BlueprintReadOnly | ) |
|
private |
Definition at line 207 of file LidarManager.h.
Referenced by DestroyTopic(), InitTopic(), and SendCombinedPointcloud().
|
private |
Definition at line 213 of file LidarManager.h.
Referenced by BeginPlay(), and SaveCombinedPointcloud().
|
private |
Definition at line 215 of file LidarManager.h.
Referenced by SaveCombinedPointcloud().
bool ALidarManager::MeasurePerformance = false |
Definition at line 147 of file LidarManager.h.
Referenced by SetMeasurePerformance(), and TickParallel().
FLidarDelegate_LidarDestroyed ALidarManager::OnLidarDestroyed |
On Lidar sensor destroyed event. BlueprintAssignable.
Definition at line 138 of file LidarManager.h.
Referenced by RemoveRaycastLidar().
FLidarDelegate_LidarSpawned ALidarManager::OnLidarSpawned |
On Lidar sensor spawned event. BlueprintAssignable.
Definition at line 131 of file LidarManager.h.
Referenced by AddRaycastLidar().
|
private |
Definition at line 217 of file LidarManager.h.
Referenced by BeginPlay(), EndPlay(), and SendCombinedPointcloud().
|
private |
Container that holds references to Lidar sensors
Definition at line 201 of file LidarManager.h.
Referenced by AddRaycastLidar(), EndPlay(), RemoveRaycastLidar(), and SimulateRaycastLidars().
|
private |
Definition at line 204 of file LidarManager.h.
Referenced by BeginPlay(), EndPlay(), InitTopic(), and SendCombinedPointcloud().
bool ALidarManager::SaveCombinedCloudToDisk = false |
Definition at line 144 of file LidarManager.h.
Referenced by ALidarManager(), SaveCombinedPointcloud(), SetSaveCombinedCloudToDisk(), and SimulateRaycastLidars().
bool ALidarManager::SendCombinedCloudToROS = true |
Definition at line 141 of file LidarManager.h.
Referenced by InitTopic(), SendCombinedPointcloud(), SetSendCombinedCloud(), and SimulateRaycastLidars().
|
private |
Definition at line 211 of file LidarManager.h.
Referenced by SetSimulateLidars(), and SimulateRaycastLidars().
|
private |
Definition at line 162 of file LidarManager.h.
Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().
|
private |
Definition at line 160 of file LidarManager.h.
Referenced by AddRaycastLidar(), EndPlay(), and RemoveRaycastLidar().
float ALidarManager::TimeInMilliseconds |
Definition at line 150 of file LidarManager.h.
Referenced by TickParallel().
|
private |
Definition at line 209 of file LidarManager.h.
Referenced by BeginPlay(), EndPlay(), RemoveRaycastLidar(), and TickParallel().