Agrarsense
CollisionSensor.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 "CollisionSensor.h"
7#include "Collision.h"
8
11
12ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
13{
14 PrimaryActorTick.bCanEverTick = false;
15}
16
17void ACollisionSensor::Init(FCollisionSensorParameters CollisionSensorParameters, bool SimulateSensor)
18{
19 // Disable this actor's collisions.
20 SetActorEnableCollision(false);
21
22 SetSimulateSensor(SimulateSensor);
23
24 if (CollisionSensorParameters.OwningActor)
25 {
26 CurrentOwner = CollisionSensorParameters.OwningActor;
27 PrimitiveComponent = CollisionSensorParameters.PrimitiveComponent;
28
29 if (CollisionSensorParameters.UseActorCollision && CurrentOwner.Get())
30 {
31 CurrentOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnActorHit);
32 }
33 else if (CollisionSensorParameters.PrimitiveComponent)
34 {
35 PrimitiveComponent->OnComponentHit.AddDynamic(this, &ACollisionSensor::OnComponentHit);
36 }
37
38 AttachToActor(CurrentOwner.Get(), FAttachmentTransformRules::KeepWorldTransform);
39 }
40
41 CollisionMessage = MakeShared<ROSMessages::std_msgs::String>();
42
45}
46
48{
49 Super::BeginPlay();
50}
51
52void ACollisionSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
53{
54 Super::EndPlay(EndPlayReason);
55
56 if (CurrentOwner.Get())
57 {
58 CurrentOwner->OnActorHit.RemoveDynamic(this, &ACollisionSensor::OnActorHit);
59 CurrentOwner.Reset();
60 }
61
63 {
64 PrimitiveComponent->OnComponentHit.RemoveDynamic(this, &ACollisionSensor::OnComponentHit);
65 PrimitiveComponent = nullptr;
66 }
67
69 CollisionMessage.Reset();
70}
71
72void ACollisionSensor::OnComponentHit(UPrimitiveComponent* HitComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp, FVector NormalImpulse, const FHitResult& Hit)
73{
74 if (!CanSimulateSensor() || !OtherActor || !CurrentOwner.Get())
75 {
76 return;
77 }
78
79 NormalImpulse *= 1e-2;
80 if (NormalImpulse.Size() > 0.3f)
81 {
82 CollisionData Collisiondata =
83 {
84 CurrentOwner.Get(),
85 OtherActor,
86 CurrentOwner->GetName(),
87 OtherActor->GetName(),
88 NormalImpulse
89 };
90
91 FString CollisionDataString = CollsionDataToString(Collisiondata);
92 SendCollisionData(CollisionDataString);
93 WriteToLogFile(CollisionDataString);
94 }
95}
96
97void ACollisionSensor::OnActorHit(AActor* Actor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit)
98{
99 if (!CanSimulateSensor() || !Actor || !OtherActor)
100 {
101 return;
102 }
103
104 NormalImpulse *= 1e-2;
105 if (NormalImpulse.Size() > 200)
106 {
107 CollisionData Collisiondata =
108 {
109 Actor,
110 OtherActor,
111 Actor->GetName(),
112 OtherActor->GetName(),
113 NormalImpulse
114 };
115
116 FString CollisionDataString = CollsionDataToString(Collisiondata);
117 SendCollisionData(CollisionDataString);
118 WriteToLogFile(CollisionDataString);
119 }
120}
121
123{
124 auto Quote = [](FString S)
125 {
126 S.ReplaceInline(TEXT("\""), TEXT("\\\""));
127 return FString::Printf(TEXT("%s"), *S);
128 };
129
130
131 FString ID;
132 FVector Location = FVector::ZeroVector;
133
134 if (Data.OtherActorPtr)
135 {
136 ID = TryGetID(Data.OtherActorPtr);
137 Location = Data.ActorPtr->GetActorLocation();
138 }
139
140 auto NormalImpulse = Data.NormalImpulse;
141 const FString ImpulseStr = FString::Printf(TEXT("%.2f,%.2f,%.2f"), NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z);
142 const double ImpulseMag = Data.NormalImpulse.Size();
143 const FString Ts = CreateTimeStampString();
144
145 return FString::Printf(
146 TEXT("actor=%s other_actor=%s other_actor_id=%s other_actor_location=%.2f,%.2f,%.2f impulse_strength=%s impulse_magnitude=%.2f timestamp=%s"),
147 *Quote(Data.Actor),
148 *Quote(Data.OtherActor),
149 *Quote(ID),
150 Location.X, Location.Y, Location.Z,
151 *ImpulseStr,
152 ImpulseMag,
153 *Quote(Ts));
154}
155
156FString ACollisionSensor::TryGetID(AActor* Actor) const
157{
158 FString ID;
159
160 if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
161 {
162 if (Actor->Implements<UActorInformation>())
163 {
164 ID = IActorInformation::Execute_GetActorID(Actor);
165 }
166 }
167
168 return ID;
169}
170
171void ACollisionSensor::SendCollisionData(const FString& CollisionDataString)
172{
173 UTopic* Topic = GetROSTopic();
174 if (IsROSConnected() && CollisionMessage.IsValid() && Topic)
175 {
176 CollisionMessage->_Data = CollisionDataString;
177 Topic->Publish(CollisionMessage);
178 }
179}
180
182{
183 if (!FileSavePath.IsEmpty())
184 {
185 return;
186 }
187
188 const FString DataLocation = UAgrarsensePaths::GetDataFolder();
189
190 // Default save path
191 FileSavePath = DataLocation + GetActorID_Implementation() + "/";
192
193 if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
194 {
195 // If sensor is attached to vehicle, save data to vehicle folder
196 FileSavePath = DataLocation + AttachedVehicle->GetActorID_Implementation();
197 }
198}
199
201{
202 if (IsValid(LogFile))
203 {
204 // File has already been created, return
205 return;
206 }
207
209
210 FLogFileSettings Settings;
213 Settings.QueueLength = MAX_int32;
214 Settings.KeepFileOpen = false;
215 Settings.Timestamp = false;
216 Settings.OverrideFilePath = true;
217 Settings.FilePath = FileSavePath;
218
219 UE_LOG(LogTemp, Warning, TEXT("FileSavePath %s"), *FileSavePath);
220
221 LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
222 if (IsValid(LogFile))
223 {
224 FString Prefix;
225 if (AVehicle* AttachedVehicle = IsAttachedToVehicle())
226 {
227 Prefix = AttachedVehicle->GetActorID_Implementation();
228 }
229 FString FileName = Prefix.IsEmpty() ? TEXT("collision") : Prefix + TEXT("_collision");
230
231 LogFile->Create(FileName, Settings);
232 }
233}
void SendCollisionData(const FString &CollisionDataString)
void Init(FCollisionSensorParameters CollisionSensorParameters, bool SimulateSensor=true)
void OnComponentHit(UPrimitiveComponent *HitComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, FVector NormalImpulse, const FHitResult &Hit)
UPrimitiveComponent * PrimitiveComponent
FString CollsionDataToString(const CollisionData &Data)
FString TryGetID(AActor *Actor) const
TSharedPtr< ROSMessages::std_msgs::String > CollisionMessage
FCollisionSensorParameters Parameters
ACollisionSensor(const FObjectInitializer &ObjectInitializer)
virtual void BeginPlay() override
void CreateLogFile() override
void CreateDataSavePath() override
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
void OnActorHit(AActor *Actor, AActor *OtherActor, FVector NormalImpulse, const FHitResult &Hit)
TWeakObjectPtr< AActor > CurrentOwner
bool CanSimulateSensor() const
Definition: Sensor.h:170
void SetSimulateSensor(bool SimulateSensor)
Definition: Sensor.h:160
UTopic * GetROSTopic() const
Definition: Sensor.h:150
AVehicle * IsAttachedToVehicle() const
Definition: Sensor.cpp:159
ULogFile * LogFile
Definition: Sensor.h:367
virtual FString GetActorID_Implementation() const override
Definition: Sensor.h:216
FString CreateTimeStampString() const
Definition: Sensor.cpp:330
FString FileSavePath
Definition: Sensor.h:372
void WriteToLogFile(const FString &Message)
Definition: Sensor.cpp:302
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:201
virtual void CreateROSTopic()
Definition: Sensor.cpp:197
static FString GetDataFolder()
void Create(const FString &FileNameWithoutExtension, FLogFileSettings Settings)
Definition: LogFile.cpp:40
AActor * OtherActorPtr
FString OtherActor
FVector NormalImpulse
AActor * ActorPtr
UPrimitiveComponent * PrimitiveComponent
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