Agrarsense
TransformSensor.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 "TransformSensor.h"
7
11
12#include "GeoReferencingSystem.h"
13#include "Kismet/GameplayStatics.h"
14#include "Engine/World.h"
15
16ATransformSensor::ATransformSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
17{
18 // TickManager handles this Actor ticking
19 PrimaryActorTick.bCanEverTick = false;
20}
21
22void ATransformSensor::Init(FTransformSensorParameters Params, bool SimulateSensor)
23{
24 SetActorEnableCollision(false);
25
26 SetSimulateSensor(SimulateSensor);
27 Parameters = Params;
28
29 ROSMessages::geometry_msgs::Transform transformMsg;
30 TransformMessage = MakeShared<ROSMessages::geometry_msgs::Transform>(transformMsg);
31
32 NavSatFixMessage = MakeShared<ROSMessages::sensor_msgs::NavSatFix>();
33 NavSatFixMessage->header.frame_id = "world";
34 NavSatFixMessage->position_covariance = {
35 1.0, 0, 0,
36 0, 1.0, 0,
37 0, 0, 1.0
38 };
39 NavSatFixMessage->position_covariance_type = NavSatFixMessage->CovarianceType::COVARIANCE_TYPE_DIAGONAL_KNOWN;
40 NavSatFixMessage->status.status = NavSatFixMessage->status.STATUS_FIX;
41 NavSatFixMessage->status.service = NavSatFixMessage->status.SERVICE_GPS;
42
45}
46
48{
49 Super::BeginPlay();
50
51 SetActorEnableCollision(false);
52
53 TickEntry = ATickManager::AddTick(this, BindTick(this, &ATransformSensor::TickParallel), ETickType::LateTickParallel);
54}
55
56void ATransformSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
57{
59
61
63 TransformMessage.Reset();
64 NavSatFixMessage.Reset();
65
66 Super::EndPlay(EndPlayReason);
67}
68
70{
71 Super::CreateROSTopic();
72
73 GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
74
75 // If map has been georeferenced, create gnss and navsatfix topics.
77 {
78 FString BaseTopicName = FString::Printf(TEXT("/agrarsense/out/sensors/%s"), *GetSensorIdentifier());
79 FString NavSatFixTopicName;
80
81 int32 LastSlashIndex = BaseTopicName.Find(TEXT("/"), ESearchCase::IgnoreCase, ESearchDir::FromEnd);
82 if (LastSlashIndex != INDEX_NONE)
83 {
84 NavSatFixTopicName = BaseTopicName.Left(LastSlashIndex) + TEXT("/navsatfix");
85 }
86 else
87 {
88 NavSatFixTopicName = BaseTopicName + TEXT("/navsatfix");
89 }
90
91 NavSatFixTopic = NewObject<UTopic>(UTopic::StaticClass());
93 {
94 NavSatFixTopic->Init(ROSInstance->ROSIntegrationCore, NavSatFixTopicName, TEXT("sensor_msgs/NavSatFix"));
95 NavSatFixTopic->Advertise();
96 }
97 }
98}
99
101{
102 if (NavSatFixTopic)
103 {
104 NavSatFixTopic->Unadvertise();
105 NavSatFixTopic->Unsubscribe();
106 NavSatFixTopic->MarkAsDisconnected();
107 NavSatFixTopic->ConditionalBeginDestroy();
108 NavSatFixTopic = nullptr;
109 }
110
111 if (ROSTopic)
112 {
113 ROSTopic->Unadvertise();
114 ROSTopic->Unsubscribe();
115 ROSTopic->MarkAsDisconnected();
116 ROSTopic->ConditionalBeginDestroy();
117 ROSTopic = nullptr;
118 }
119}
120
122{
123 if (!CanSimulateSensor())
124 {
125 return;
126 }
127
128 FTransform CurrentTransform;
129 const bool HasValidTransform =
130 (Parameters.UseOwningActorTransform && Parameters.OwningActor && (CurrentTransform = Parameters.OwningActor->GetTransform(), true)) ||
131 (Parameters.PrimitiveComponent && Parameters.UsePrimiteRelativeTransform && (CurrentTransform = Parameters.PrimitiveComponent->GetRelativeTransform(), true)) ||
132 (Parameters.PrimitiveComponent && !Parameters.UsePrimiteRelativeTransform && (CurrentTransform = Parameters.PrimitiveComponent->GetComponentTransform(), true));
133
134 if (!HasValidTransform)
135 {
136 return;
137 }
138
139 const FVector CurrentPosition = CurrentTransform.GetLocation();
140 const FQuat CurrentRotation = CurrentTransform.GetRotation();
141
142 const float PositionToleranceSq = 0.01f;
143 const float RotationTolerance = 0.001f;
144
145 const bool PositionChanged = FVector::DistSquared(CurrentPosition, PreviousPosition) > PositionToleranceSq;
146 const bool RotationChanged = !CurrentRotation.Equals(PreviousRotation, RotationTolerance);
147
148 TimeSinceLastPublish += DeltaTime;
149
150 if (PositionChanged || RotationChanged || TimeSinceLastPublish >= 1.0f)
151 {
153 PreviousPosition = CurrentPosition;
154 PreviousRotation = CurrentRotation;
155
156 if (IsROSConnected())
157 {
158 SendTransformDataToROS(CurrentPosition, CurrentRotation);
159 SendGpsDataToROS(CurrentPosition);
160 }
161
162 SaveTransformMetaDataToDisk(CurrentPosition, CurrentRotation);
163 }
164}
165
166void ATransformSensor::SendTransformDataToROS(const FVector& Translation, const FQuat& Rotation)
167{
168 UTopic* Topic = GetROSTopic();
169 if (Topic && TransformMessage.IsValid())
170 {
171 TransformMessage->translation = Translation;
172 TransformMessage->rotation = Rotation;
173
174 Topic->Publish(TransformMessage);
175 }
176}
177
178void ATransformSensor::SendGpsDataToROS(const FVector& ActorPosition)
179{
181 {
182 return;
183 }
184
185 // Convert Actor position to geographic coordinates
186 FGeographicCoordinates GeographicCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
187 const double Latitude = GeographicCoordinates.Latitude;
188 const double Longitude = GeographicCoordinates.Longitude;
189 const double Altitude = GeographicCoordinates.Altitude;
190
191 // gps data as NavSatFix ROS message
192 if (NavSatFixTopic && NavSatFixMessage.IsValid())
193 {
194 NavSatFixMessage->latitude = Latitude;
195 NavSatFixMessage->longitude = Longitude;
196 NavSatFixMessage->altitude = Altitude;
197 NavSatFixMessage->header.time = FROSTime::Now();
198
200 }
201}
202
203void ATransformSensor::SaveTransformMetaDataToDisk(const FVector& ActorPosition, const FQuat& Rotation)
204{
206 {
207 return;
208 }
209
210 const FRotator ActorRotation = Rotation.Rotator();
211
212 FString MetaData;
213
214 FString TimeStamp = CreateTimeStampString();
215
217 {
218 FGeographicCoordinates GeographicCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
219 const double Latitude = GeographicCoordinates.Latitude;
220 const double Longitude = GeographicCoordinates.Longitude;
221 const double Altitude = GeographicCoordinates.Altitude;
222
223 MetaData = FString::Printf(TEXT("%s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.8f, %.8f, %.8f"), *TimeStamp, ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
224 ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll,
225 GeographicCoordinates.Latitude, GeographicCoordinates.Longitude, GeographicCoordinates.Altitude);
226 }
227 else
228 {
229 MetaData = FString::Printf(TEXT("%s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f"), *TimeStamp, ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
230 ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll);
231 }
232
233 // Write to the log file (this is written after sensor is destroyed)
234 WriteToLogFile(MetaData);
235}
236
238{
240 {
241 return;
242 }
243
245
246 FLogFileSettings Settings;
249 Settings.QueueLength = MAX_int32; // Only write the log after destroying the sensor
250 Settings.KeepFileOpen = false;
251 Settings.Timestamp = false;
252 Settings.OverrideFilePath = true;
253 Settings.FilePath = FileSavePath;
254
255 LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
256 if (IsValid(LogFile))
257 {
258 FString Prefix;
260 {
261 Prefix = Vehicle->GetActorID_Implementation();
262 }
263 FString FileName = Prefix.IsEmpty() ? TEXT("transform") : Prefix + TEXT("_transform");
264
265 LogFile->Create(FileName, Settings);
266
267 AGeoReferencingSystem* GeoRef = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
268 if (GeoRef)
269 {
270 WriteToLogFile("timestamp, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation, GPS latitude, GPS longitude, GPS altitude");
271 }
272 else
273 {
274 WriteToLogFile("timestamp, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation");
275 }
276 }
277}
278
280{
281 if (!FileSavePath.IsEmpty())
282 {
283 return;
284 }
285
286 const FString DataLocation = UAgrarsensePaths::GetDataFolder();
287
288 // Default save path
289 FileSavePath = DataLocation + GetActorID_Implementation() + "/";
290
292 {
293 // If sensor is attached to vehicle, save data to vehicle folder
294 const FString VehicleName = Vehicle->GetActorID_Implementation();
295 FileSavePath = DataLocation + VehicleName;
296 }
297 else if (ParentActor)
298 {
299 if (ASensor* ParentSensor = Cast<ASensor>(ParentActor))
300 {
301 if (AVehicle* SensorVehicle = ParentSensor->IsAttachedToVehicle())
302 {
303 // If this TransformSensor is attached to a sensor that's attached to a vehicle,
304 // save data to Vehicle/Sensor folder
305 const FString VehicleName = SensorVehicle->GetActorID_Implementation();
306 const FString ParentSensorName = ParentSensor->GetActorID_Implementation();
307 FileSavePath = DataLocation + VehicleName + "/" + ParentSensorName;
308 }
309 }
310 }
311}
static auto BindTick(ObjectType *Object, FunctionType Function)
Definition: TickManager.h:162
Definition: Sensor.h:45
UROSIntegrationGameInstance * ROSInstance
Definition: Sensor.h:375
bool CanSimulateSensor() const
Definition: Sensor.h:170
FString GetSensorIdentifier() const
Definition: Sensor.h:75
UTopic * ROSTopic
Definition: Sensor.h:361
AActor * ParentActor
Definition: Sensor.h:370
void SetSimulateSensor(bool SimulateSensor)
Definition: Sensor.h:160
UTopic * GetROSTopic() const
Definition: Sensor.h:150
AVehicle * IsAttachedToVehicle() const
Definition: Sensor.cpp:152
ULogFile * LogFile
Definition: Sensor.h:367
virtual FString GetActorID_Implementation() const override
Definition: Sensor.h:216
FString CreateTimeStampString() const
Definition: Sensor.cpp:323
FString FileSavePath
Definition: Sensor.h:372
void WriteToLogFile(const FString &Message)
Definition: Sensor.cpp:295
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:201
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
void SendTransformDataToROS(const FVector &Translation, const FQuat &Rotation)
void CreateDataSavePath() override
virtual void BeginPlay() override
void SendGpsDataToROS(const FVector &ActorPosition)
UTopic * NavSatFixTopic
TSharedPtr< ROSMessages::geometry_msgs::Transform > TransformMessage
void SaveTransformMetaDataToDisk(const FVector &Translation, const FQuat &Rotation)
void CreateLogFile() override
TSharedPtr< ROSMessages::sensor_msgs::NavSatFix > NavSatFixMessage
virtual void CreateROSTopic() override
FVector PreviousPosition
FTransformSensorParameters Parameters
virtual void DestroyROSTopic() override
AGeoReferencingSystem * GeoReferencingSystem
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
FTickEntry TickEntry
void TickParallel(float DeltaTime)
void Init(FTransformSensorParameters Params, bool SimulateSensor=true)
ATransformSensor(const FObjectInitializer &ObjectInitializer)
static FString GetDataFolder()
static FGeographicCoordinates UnrealToGeographicCoordinates(AGeoReferencingSystem *GeoReferencingSystem, const FVector &Position)
void Create(const FString &FileNameWithoutExtension, FLogFileSettings Settings)
Definition: LogFile.cpp:40
bool KeepFileOpen
Definition: LogFile.h:42
bool Timestamp
Definition: LogFile.h:39
FString FilePath
Definition: LogFile.h:54
FFileWriteOptions FileWriteOptions
Definition: LogFile.h:45
int32 QueueLength
Definition: LogFile.h:48
bool OverrideFilePath
Definition: LogFile.h:51
FFileCreationOptions FileCreationOptions
Definition: LogFile.h:36
UPrimitiveComponent * PrimitiveComponent