Safety bubble (#14)

The NCP will stop when it enters a shape component around an entity with `SafetyBubble` tag. --------- Signed-off-by: Michał Pełka <[email protected]> Co-authored-by: Artur Kamieniecki <[email protected]>
RobotecAI · Oct 4, 2024 · 5f28b9a · 5f28b9a
1 parent dd619b7
commit 5f28b9a
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git 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
@@ -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();