Agrarsense
OverlapSensor.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 "OverlapSensor.h"
8
12
13#if WITH_EDITOR
14#include "DrawDebugHelpers.h"
15#endif
16
17AOverlapSensor::AOverlapSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
18{
19 PrimaryActorTick.bCanEverTick = false;
20
21 BoundsComponent = CreateDefaultSubobject<UBoundsVisualizerComponent>(TEXT("BoundsComponent"));
22 BoundsComponent->SetupAttachment(RootComponent);
23 BoundsComponent->SetGenerateOverlapEvents(true);
24 BoundsComponent->SetVisibility(false);
25 BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
26}
27
29{
30 Parameters = InParameters;
31
32 Vehicle = Cast<AVehicle>(Parameters.OwningActor);
33
34 if (Vehicle)
35 {
36 VehicleName = IActorInformation::Execute_GetActorName(Vehicle);
37 VehicleID = IActorInformation::Execute_GetActorID(Vehicle);
38 }
39
40 UPrimitiveComponent* TriggerComponent = Cast<UPrimitiveComponent>(BoundsComponent);
41
42 if (!InParameters.AllChannels)
43 {
44 const int32 OverlapSensorIndex = 2;
45 const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
46
47 BoundsComponent->SetCollisionObjectType(OverlapSensorChannel);
48 BoundsComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
49 BoundsComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
50 }
51 else
52 {
53 // Set to use custom collision profile defined in DefaultEngine.ini
54 BoundsComponent->SetCollisionProfileName(TEXT("OverlapSensorAll"));
55 }
56
57 BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
58 BoundsComponent->SetGenerateOverlapEvents(true);
59 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
60
61 if (RootComponent)
62 {
63 BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
64 }
65
66 TriggerComponent = BoundsComponent;
67
68 if (TriggerComponent)
69 {
70 SetOverlapBounds(InParameters.Size);
71 //SetRootComponent(TriggerComponent);
73
74 ROSMessages::std_msgs::String StringMsg;
75 ROSMessage = MakeShared<ROSMessages::std_msgs::String>(StringMsg);
76
78 {
80 }
81 }
82
84 {
85 FVector Size = FVector(Parameters.Size.X, Parameters.Size.Y, Parameters.Size.Z);
86 BoundsComponent->SetRelativeScale3D(Size);
87 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
88
89 if (RootComponent)
90 {
91 BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
92 }
93 }
94
96 {
97 AttachToActor(Parameters.OwningActor, FAttachmentTransformRules::KeepWorldTransform);
98 }
99
101}
102
104{
105 Super::BeginPlay();
106
107 BoundsComponent->OnComponentBeginOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapBegin);
108 BoundsComponent->OnComponentEndOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapEnd);
109}
110
111void AOverlapSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
112{
113 Super::EndPlay(EndPlayReason);
114
115 BoundsComponent->OnComponentBeginOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapBegin);
116 BoundsComponent->OnComponentEndOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapEnd);
117}
118
119void AOverlapSensor::SetOverlapBounds(const FVector& NewSize)
120{
121 Parameters.Size = NewSize;
122
123 if (BoundsComponent)
124 {
125 FVector Size = FVector(Parameters.Size.X, Parameters.Size.Y , Parameters.Size.Z);
126 BoundsComponent->SetRelativeScale3D(Size);
127 }
128
129 FString ActorID = IActorInformation::Execute_GetActorID(this);
130
131 FString Msg = FString::Printf(TEXT("OverlapSensor with ID: %s changed overlap bounds to: %s"), *ActorID, *NewSize.ToString());
133}
134
136{
137 IsVisible = Visualize;
138
139 if (BoundsComponent)
140 {
141 BoundsComponent->SetVisibility(IsVisible);
142 }
143}
144
146{
148
149 if (BoundsComponent)
150 {
151 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
152 }
153
154 FString Msg = FString::Printf(TEXT("OverlapSensor.cpp: Changed overlap relative position to: %s"), *Vector.ToString());
156}
157
158void AOverlapSensor::SetOverlapResponseToAllChannels(const ECollisionResponse Response)
159{
160 if (Response != ECollisionResponse::ECR_Overlap && Response != ECollisionResponse::ECR_Ignore)
161 {
162 // If Response is neither ECR_Overlap nor ECR_Ignore, return
163 return;
164 }
165
166 if (BoundsComponent)
167 {
168 BoundsComponent->SetCollisionResponseToAllChannels(Response);
169 }
170}
171
172void AOverlapSensor::OnOverlapBegin(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp,
173 int32 OtherBodyIndex, bool bFromSweep, const FHitResult& SweepResult)
174{
175 // Don't allow Overlapping with other Overlapping sensors..
176 AOverlapSensor* OtherOverlapSensor = Cast<AOverlapSensor>(OtherActor);
177 if (OtherOverlapSensor)
178 {
179 return;
180 }
181
182 BuildAndSendMessage("Overlap Begin with", OtherActor);
183}
184
185void AOverlapSensor::OnOverlapEnd(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor,
186 UPrimitiveComponent* OtherComp, int32 OtherBodyIndex)
187{
188 // Don't allow Overlapping with other Overlapping sensors..
189 AOverlapSensor* OtherOverlapSensor = Cast< AOverlapSensor>(OtherActor);
190 if (OtherOverlapSensor)
191 {
192 return;
193 }
194
195 BuildAndSendMessage("Overlap End with", OtherActor);
196}
197
198void AOverlapSensor::BuildAndSendMessage(const FString& Prefix, AActor* Actor)
199{
200 if (!Actor)
201 {
202 return;
203 }
204
205 FString ID = TryGetID(Actor);
206 FString Message;
207
208 // Calculate distance from sensor parent (vehicle) to incoming Actor
209 AActor* From = this;
210 if (AActor* VehicleActorPtr = GetParentActor())
211 {
212 From = VehicleActorPtr;
213 }
214 float DistanceInMeters = FVector::Dist(From->GetActorLocation(), Actor->GetActorLocation()) / 100.0f;
215
216 // Get Actor Location
217 FVector Location = Actor->GetActorLocation();
218
219 // Format location info
220 FString LocationStr = FString::Printf(TEXT("Location: (%.2f, %.2f, %.2f)"),
221 Location.X, Location.Y, Location.Z);
222
223 // Add distance and location to the message
224 if (ID.IsEmpty())
225 {
226 Message = FString::Printf(TEXT("%s: %s, Distance: %.2f m, %s"),
227 *Prefix, *Actor->GetName(), DistanceInMeters, *LocationStr);
228 }
229 else
230 {
231 Message = FString::Printf(TEXT("%s: %s: ID: %s, Distance: %.2f m, %s"),
232 *Prefix, *Actor->GetName(), *ID, DistanceInMeters, *LocationStr);
233 }
234
235 UTopic* Topic = GetROSTopic();
236 if (ROSMessage.IsValid() && Topic && IsROSConnected())
237 {
238 ROSMessage->_Data = Message;
239 Topic->Publish(ROSMessage);
240 }
241
242 WriteToLogFile(Message);
243
244 if (Vehicle)
245 {
246 FString Msg = FString::Printf(TEXT("Actor '%s' with ID: '%s' %s"), *VehicleName, *VehicleID, *Message);
248 }
249}
250
252{
253 if (!FileSavePath.IsEmpty())
254 {
255 return;
256 }
257
258 const FString DataLocation = UAgrarsensePaths::GetDataFolder();
259
260 // Default save path
261 FileSavePath = DataLocation + GetActorID_Implementation() + "/";
262
263 if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
264 {
265 // If sensor is attached to vehicle, save data to vehicle folder
266 FileSavePath = DataLocation + AttachedVehicle->GetActorID_Implementation();
267 }
268}
269
271{
272 if (IsValid(LogFile))
273 {
274 // File has already been created, return
275 return;
276 }
277
279
280 FLogFileSettings Settings;
283 Settings.QueueLength = MAX_int32;
284 Settings.KeepFileOpen = false;
285 Settings.Timestamp = false;
286 Settings.OverrideFilePath = true;
287 Settings.FilePath = FileSavePath;
288
289 UE_LOG(LogTemp, Warning, TEXT("FileSavePath %s"), *FileSavePath);
290
291 LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
292 if (IsValid(LogFile))
293 {
294 FString Prefix;
295 if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
296 {
297 Prefix = AttachedVehicle->GetActorID_Implementation();
298 }
299 FString FileName = Prefix.IsEmpty() ? TEXT("overlap") : Prefix + TEXT("_overlap");
300
301 LogFile->Create(FileName, Settings);
302
303 }
304}
305
306FString AOverlapSensor::TryGetID(AActor* Actor)
307{
308 FString ID;
309
310 if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
311 {
312 if (Actor->Implements<UActorInformation>())
313 {
314 ID = IActorInformation::Execute_GetActorID(Actor);
315 }
316 }
317
318 return ID;
319}
void OnOverlapEnd(UPrimitiveComponent *OverlappedComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex)
void CreateDataSavePath() override
void SetOverlapRelativePosition(const FVector &Vector)
TSharedPtr< ROSMessages::std_msgs::String > ROSMessage
AOverlapSensor(const FObjectInitializer &ObjectInitializer)
FString VehicleName
void CreateLogFile() override
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
virtual void BeginPlay() override
UBoundsVisualizerComponent * BoundsComponent
Definition: OverlapSensor.h:98
void Init(FOverlapSensorParameters InParameters)
void SetVisualizeOverlapArea(bool Visualize)
void SetOverlapBounds(const FVector &NewSize)
FOverlapSensorParameters Parameters
void BuildAndSendMessage(const FString &Prefix, AActor *Actor)
void SetOverlapResponseToAllChannels(const ECollisionResponse Response)
AVehicle * Vehicle
FString TryGetID(AActor *Actor)
void OnOverlapBegin(UPrimitiveComponent *OverlappedComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex, bool bFromSweep, const FHitResult &SweepResult)
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 FileSavePath
Definition: Sensor.h:372
void WriteToLogFile(const FString &Message)
Definition: Sensor.cpp:295
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:201
virtual void CreateROSTopic()
Definition: Sensor.cpp:190
static void Log(const FString &Message, bool LogToTextFile=true, bool LogToROS=true)
static FString GetDataFolder()
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