From 5f28b9a65aa2f7df485db6d73838fb79488b3830 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= <michal.pelka@robotec.ai>
Date: Fri, 4 Oct 2024 12:07:54 +0200
Subject: [PATCH] Safety bubble (#14)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The NCP will stop when it enters a shape component around an entity with `SafetyBubble` tag.

---------

Signed-off-by: Michał Pełka <michal.pelka@robotec.ai>
Co-authored-by: Artur Kamieniecki <artur@arturkamieniecki.pl>
---
 .../HumanWorker/NpcNavigatorComponent.cpp     | 20 +++++++++++++++++++
 .../HumanWorker/NpcNavigatorComponent.h       |  2 ++
 2 files changed, 22 insertions(+)

diff --git a/Code/Source/HumanWorker/NpcNavigatorComponent.cpp b/Code/Source/HumanWorker/NpcNavigatorComponent.cpp
index 1953ae8..711d626 100644
--- a/Code/Source/HumanWorker/NpcNavigatorComponent.cpp
+++ b/Code/Source/HumanWorker/NpcNavigatorComponent.cpp
@@ -26,6 +26,8 @@
 #include <ROS2/Utilities/ROS2Names.h>
 #include <RecastNavigation/DetourNavigationBus.h>
 #include <RecastNavigation/RecastNavigationMeshBus.h>
+#include <LmbrCentral/Shape/ShapeComponentBus.h>
+#include <LmbrCentral/Scripting/TagComponentBus.h>
 
 namespace ROS2::HumanWorker
 {
@@ -193,6 +195,24 @@ namespace ROS2::HumanWorker
         const AZ::Vector3 RobotPosition = currentTransform.GetTranslation();
         const AZ::Vector3 RobotDirection = currentTransform.GetBasisX().GetNormalized();
 
+
+        // get deny bubble entities
+        AZ::EBusAggregateResults<AZ::EntityId> aggregator;
+        LmbrCentral::TagGlobalRequestBus::EventResult(aggregator, SafetyBubbleTag, &LmbrCentral::TagGlobalRequests::RequestTaggedEntities);
+
+        for (const AZ::EntityId& denyBubbleEntity : aggregator.values)
+        {
+            bool isInside = false;
+
+            LmbrCentral::ShapeComponentRequestsBus::EventResult(
+                isInside, denyBubbleEntity, &LmbrCentral::ShapeComponentRequests::IsPointInside, RobotPosition);
+            if (isInside)
+            {
+                return { .m_linear = 0.0f, .m_angular = 0.0f };
+            }
+        }
+
+
         float bearingError = 0.0f;
         if (goal.m_position != RobotPosition)
         {
diff --git a/Code/Source/HumanWorker/NpcNavigatorComponent.h b/Code/Source/HumanWorker/NpcNavigatorComponent.h
index a2e371c..9d0e1a7 100644
--- a/Code/Source/HumanWorker/NpcNavigatorComponent.h
+++ b/Code/Source/HumanWorker/NpcNavigatorComponent.h
@@ -34,6 +34,8 @@ namespace ROS2::HumanWorker
         , private RecastNavigation::RecastNavigationMeshNotificationBus::Handler
     {
     public:
+        static constexpr  AZ::Crc32 SafetyBubbleTag = AZ_CRC_CE("SafetyBubble");
+
         AZ_RTTI(NpcNavigatorComponent, "{BA4E4F96-A88C-406B-853B-E05911D190C4}", AZ::Component);
 
         NpcNavigatorComponent();