Added option for setting spline points in local or offseted coordinat…

…e system.
RobotecAI · Nov 27, 2024 · d7ee717 · d7ee717
1 parent f2086ed
commit d7ee717
Show file tree

Hide file tree

Showing 4 changed files with 55 additions and 3 deletions.
diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp
@@ -3,6 +3,7 @@
 #include <AzCore/Component/TransformBus.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <LmbrCentral/Scripting/TagComponentBus.h>
 #include <LmbrCentral/Shape/SplineComponentBus.h>
 #include <ROS2/Georeference/GeoreferenceBus.h>
 
@@ -54,6 +55,32 @@ namespace SplineTools
             });
     }
 
+    bool SplineSubscriber::GetOffsetTransform(AZ::Transform& transform)
+    {
+        AZ::EBusAggregateResults<AZ::EntityId> aggregator;
+        const LmbrCentral::Tag tag = AZ::Crc32(m_config.m_startOffsetTag);
+        LmbrCentral::TagGlobalRequestBus::EventResult(aggregator, tag, &LmbrCentral::TagGlobalRequests::RequestTaggedEntities);
+
+        AZ_Warning(
+            "SplineSubscriber",
+            aggregator.values.size() <= 1,
+            "Multiple entities found with tag %s. The first entity will be used.",
+            m_config.m_startOffsetTag.c_str());
+
+        AZ_Warning("SplineSubscriber", !aggregator.values.empty(), "No entity with tag found %s.", m_config.m_startOffsetTag.c_str());
+
+        if (!aggregator.values.empty())
+        {
+            AZ::Transform offsetTransform = AZ::Transform::CreateIdentity();
+            const AZ::EntityId& entityId = aggregator.values[0];
+            AZ::TransformBus::EventResult(offsetTransform, entityId, &AZ::TransformBus::Events::GetWorldTM);
+            transform = offsetTransform;
+            return true;
+        }
+
+        return false;
+    }
+
     void SplineSubscriber::OnSplineReceived(const nav_msgs::msg::Path& msg)
     {
         AZ::SplinePtr splinePtr;
@@ -64,6 +91,10 @@ namespace SplineTools
         AZ::Transform worldTm = AZ::Transform::CreateIdentity();
         AZ::TransformBus::EventResult(worldTm, GetEntityId(), &AZ::TransformBus::Events::GetWorldTM);
         worldTm.Invert();
+
+        AZ::Transform offsetTransform = AZ::Transform::CreateIdentity();
+        auto hasOffset = GetOffsetTransform(offsetTransform);
+
         AZStd::string frame{ msg.header.frame_id.c_str(), msg.header.frame_id.size() };
         AZStd::to_upper(frame.begin(), frame.end());
         AZ_Printf("SplineSubscriber", "Frame: %s", frame.data());
@@ -76,7 +107,19 @@ namespace SplineTools
 
             if (frame.empty())
             {
-                points[i] = worldTm.TransformPoint(posePoint);
+                if (hasOffset)
+                {
+                    auto referenceTranslation = offsetTransform.TransformPoint(posePoint);
+                    points[i] = worldTm.TransformPoint(referenceTranslation);
+                }
+                else
+                {
+                    points[i] = worldTm.TransformPoint(posePoint);
+                }
+            }
+            else if (frame == "LOCAL")
+            {
+                points[i] = posePoint;
             }
             else if (frame == "WGS84" && m_config.m_allowWGS84)
             {

diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h
@@ -2,6 +2,7 @@
 
 #include "SplineSubscriberConfig.h"
 #include <AzCore/Component/Component.h>
+#include <AzCore/Math/Transform.h>
 #include <ROS2/ROS2Bus.h>
 #include <SplineTools/SplineToolsTypeIds.h>
 #include <nav_msgs/msg/path.hpp>
@@ -20,6 +21,7 @@ namespace SplineTools
         void Deactivate() override;
 
     private:
+        bool GetOffsetTransform(AZ::Transform& transform);
         void OnSplineReceived(const nav_msgs::msg::Path& msg);
         SplineSubscriberConfiguration m_config;
         rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr m_subscription;

diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.cpp
@@ -19,7 +19,8 @@ namespace SplineTools
                 ->Version(0)
                 ->Field("m_topicName", &SplineSubscriberConfiguration::m_topic)
                 ->Field("m_allowWGS84", &SplineSubscriberConfiguration::m_allowWGS84)
-                ->Field("m_resetOnActivation", &SplineSubscriberConfiguration::m_resetOnActivation);
+                ->Field("m_resetOnActivation", &SplineSubscriberConfiguration::m_resetOnActivation)
+                ->Field("m_startOffsetTag", &SplineSubscriberConfiguration::m_startOffsetTag);
             if (auto editContext = serializeContext->GetEditContext())
             {
                 editContext
@@ -32,7 +33,12 @@ namespace SplineTools
                         AZ::Edit::UIHandlers::Default,
                         &SplineSubscriberConfiguration::m_resetOnActivation,
                         "Reset On Activation",
-                        "Reset On Activation");
+                        "Reset On Activation")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &SplineSubscriberConfiguration::m_startOffsetTag,
+                        "Start Offset Tag",
+                        "Tag that will be used to set the start offset for the spline.");
             }
         }
     }

diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.h b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.h
@@ -17,5 +17,6 @@ namespace SplineTools
         ROS2::TopicConfiguration m_topic{ rclcpp::ServicesQoS() };
         bool m_allowWGS84{ true }; //! Allow WGS84 coordinates.
         bool m_resetOnActivation{ true }; //! Reset entity's spline on activation.
+        AZStd::string m_startOffsetTag; //! Tag of the entity to use for the spline points offset.
     };
 } // namespace SplineTools