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
17static constexpr float SIZE_DIVISOR = 70.0f;
18
19AOverlapSensor::AOverlapSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
20{
21 PrimaryActorTick.bCanEverTick = false;
22
23 BoundsComponent = CreateDefaultSubobject<UBoundsVisualizerComponent>(TEXT("BoundsComponent"));
24 BoundsComponent->SetupAttachment(RootComponent);
25 BoundsComponent->SetGenerateOverlapEvents(true);
26 BoundsComponent->SetVisibility(false);
27 BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
28}
29
31{
32 Parameters = InParameters;
33
34 Vehicle = Cast<AVehicle>(Parameters.OwningActor);
35
36 if (Vehicle)
37 {
38 VehicleName = IActorInformation::Execute_GetActorName(Vehicle);
39 VehicleID = IActorInformation::Execute_GetActorID(Vehicle);
40 }
41
42 UPrimitiveComponent* TriggerComponent = Cast<UPrimitiveComponent>(BoundsComponent);
43
44 // if you change DefaultEngine.ini collision channels then this index must be changed as well!
45 const int32 OverlapSensorIndex = 2;
46 const ECollisionChannel OverlapSensorChannel = UCollisionProfile::Get()->ConvertToCollisionChannel(false, OverlapSensorIndex);
47 BoundsComponent->SetCollisionEnabled(ECollisionEnabled::QueryOnly);
48 BoundsComponent->SetGenerateOverlapEvents(true);
49 BoundsComponent->SetCollisionObjectType(OverlapSensorChannel);
50 BoundsComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Ignore);
51 BoundsComponent->SetCollisionResponseToChannel(OverlapSensorChannel, ECollisionResponse::ECR_Overlap);
52 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
53
54 if (RootComponent)
55 {
56 BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
57 }
58
59 TriggerComponent = BoundsComponent;
60
61 if (TriggerComponent)
62 {
63 SetOverlapBounds(InParameters.Size);
64 //SetRootComponent(TriggerComponent);
66
67 ROSMessages::std_msgs::String StringMsg;
68 ROSMessage = MakeShared<ROSMessages::std_msgs::String>(StringMsg);
69
71 {
73 }
74 }
75
77 {
78 // Adjust the scale of the Sphere/Box component bounds to match the scale of the VisualizerComponent mesh
79 FVector ScaledVector = FVector(Parameters.Size.X / SIZE_DIVISOR, Parameters.Size.Y / SIZE_DIVISOR, Parameters.Size.Z / SIZE_DIVISOR);
80 BoundsComponent->SetRelativeScale3D(ScaledVector);
81 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
82
83 if (RootComponent)
84 {
85 BoundsComponent->AttachToComponent(RootComponent, FAttachmentTransformRules::KeepWorldTransform);
86 }
87 }
88
90 {
91 AttachToActor(Parameters.OwningActor, FAttachmentTransformRules::KeepWorldTransform);
92 }
93}
94
96{
97 Super::BeginPlay();
98
99 BoundsComponent->OnComponentBeginOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapBegin);
100 BoundsComponent->OnComponentEndOverlap.AddDynamic(this, &AOverlapSensor::OnOverlapEnd);
101}
102
103void AOverlapSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
104{
105 Super::EndPlay(EndPlayReason);
106
107 BoundsComponent->OnComponentBeginOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapBegin);
108 BoundsComponent->OnComponentEndOverlap.RemoveDynamic(this, &AOverlapSensor::OnOverlapEnd);
109}
110
111void AOverlapSensor::SetOverlapBounds(const FVector& NewSize)
112{
113 Parameters.Size = NewSize;
114
115 if (BoundsComponent)
116 {
117 // Adjust the scale of the Sphere/Box component bounds to match the scale of the VisualizerComponent mesh
118 FVector ScaledVector = FVector(Parameters.Size.X / SIZE_DIVISOR, Parameters.Size.Y / SIZE_DIVISOR, Parameters.Size.Z / SIZE_DIVISOR);
119 BoundsComponent->SetRelativeScale3D(ScaledVector);
120 }
121
122 FString ActorID = IActorInformation::Execute_GetActorID(this);
123
124 FString Msg = FString::Printf(TEXT("OverlapSensor with ID: %s changed overlap bounds to: %s"), *ActorID, *NewSize.ToString());
126}
127
129{
130 IsVisible = Visualize;
131
132 if (BoundsComponent)
133 {
134 BoundsComponent->SetVisibility(IsVisible);
135 }
136}
137
139{
141
142 if (BoundsComponent)
143 {
144 BoundsComponent->SetRelativeLocation(Parameters.RelativePosition);
145 }
146
147 FString Msg = FString::Printf(TEXT("OverlapSensor.cpp: Changed overlap relative position to: %s"), *Vector.ToString());
149}
150
151void AOverlapSensor::SetOverlapResponseToAllChannels(const ECollisionResponse Response)
152{
153 if (Response != ECollisionResponse::ECR_Overlap && Response != ECollisionResponse::ECR_Ignore)
154 {
155 // If Response is neither ECR_Overlap nor ECR_Ignore, return
156 return;
157 }
158
159 if (BoundsComponent)
160 {
161 BoundsComponent->SetCollisionResponseToAllChannels(Response);
162 }
163}
164
165void AOverlapSensor::OnOverlapBegin(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor, UPrimitiveComponent* OtherComp,
166 int32 OtherBodyIndex, bool bFromSweep, const FHitResult& SweepResult)
167{
168 // Don't allow Overlapping with other Overlapping sensors..
169 AOverlapSensor* OtherOverlapSensor = Cast<AOverlapSensor>(OtherActor);
170 if (OtherOverlapSensor)
171 {
172 return;
173 }
174
175 BuildAndSendMessage("Overlap Begin with", OtherActor);
176}
177
178void AOverlapSensor::OnOverlapEnd(UPrimitiveComponent* OverlappedComponent, AActor* OtherActor,
179 UPrimitiveComponent* OtherComp, int32 OtherBodyIndex)
180{
181 // Don't allow Overlapping with other Overlapping sensors..
182 AOverlapSensor* OtherOverlapSensor = Cast< AOverlapSensor>(OtherActor);
183 if (OtherOverlapSensor)
184 {
185 return;
186 }
187
188 BuildAndSendMessage("Overlap End with", OtherActor);
189}
190
191void AOverlapSensor::BuildAndSendMessage(const FString& Prefix, AActor* Actor)
192{
193 if (!Actor)
194 {
195 return;
196 }
197
198 FString ID = TryGetID(Actor);
199 FString Message;
200
201 // Calculate distance from sensor parent (vehicle) to incoming Actor
202 AActor* From = this;
203 if (AActor* VehicleActorPtr = GetParentActor())
204 {
205 From = VehicleActorPtr;
206 }
207 float DistanceInMeters = FVector::Dist(From->GetActorLocation(), Actor->GetActorLocation()) / 100.0f;
208
209 // Add distance to the end of the message
210 if (ID.IsEmpty())
211 {
212 Message = FString::Printf(TEXT("%s: %s, Distance: %.2f m"), *Prefix, *Actor->GetName(), DistanceInMeters);
213 }
214 else
215 {
216 Message = FString::Printf(TEXT("%s: %s: ID: %s, Distance: %.2f m"), *Prefix, *Actor->GetName(), *ID, DistanceInMeters);
217 }
218
219 UTopic* Topic = GetROSTopic();
220 if (ROSMessage.IsValid() && Topic && IsROSConnected())
221 {
222 ROSMessage->_Data = Message;
223 Topic->Publish(ROSMessage);
224 }
225
226 WriteToLogFile(Message);
227
228 if (Vehicle)
229 {
230 FString Msg = FString::Printf(TEXT("Actor '%s' with ID: '%s' %s"), *VehicleName, *VehicleID, *Message);
232 }
233}
234
235FString AOverlapSensor::TryGetID(AActor* Actor)
236{
237 FString ID;
238
239 if (Actor && Actor->GetClass()->ImplementsInterface(UActorInformation::StaticClass()))
240 {
241
242 if (Actor->Implements<UActorInformation>())
243 {
244 ID = IActorInformation::Execute_GetActorID(Actor);
245 }
246 }
247
248 return ID;
249}
static constexpr float SIZE_DIVISOR
void OnOverlapEnd(UPrimitiveComponent *OverlappedComponent, AActor *OtherActor, UPrimitiveComponent *OtherComp, int32 OtherBodyIndex)
void SetOverlapRelativePosition(const FVector &Vector)
TSharedPtr< ROSMessages::std_msgs::String > ROSMessage
AOverlapSensor(const FObjectInitializer &ObjectInitializer)
FString VehicleName
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:151
UTopic * GetROSTopic() const
Definition: Sensor.h:141
void WriteToLogFile(const FString &Message)
Definition: Sensor.cpp:269
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:192
virtual void CreateROSTopic()
Definition: Sensor.cpp:173
static void Log(const FString &Message, bool LogToTextFile=true, bool LogToROS=true)