Small refactor

Signed-off-by: Michał Pełka <[email protected]>
RobotecAI · Nov 28, 2024 · c2c2dae · c2c2dae
1 parent d7ee717
commit c2c2dae
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 27 deletions.
diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp
@@ -55,7 +55,7 @@ namespace SplineTools
             });
     }
 
-    bool SplineSubscriber::GetOffsetTransform(AZ::Transform& transform)
+    AZ::Transform SplineSubscriber::GetOffsetTransform()
     {
         AZ::EBusAggregateResults<AZ::EntityId> aggregator;
         const LmbrCentral::Tag tag = AZ::Crc32(m_config.m_startOffsetTag);
@@ -74,11 +74,10 @@ namespace SplineTools
             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 offsetTransform;
         }
 
-        return false;
+        return AZ::Transform::CreateIdentity();
     }
 
     void SplineSubscriber::OnSplineReceived(const nav_msgs::msg::Path& msg)
@@ -93,7 +92,7 @@ namespace SplineTools
         worldTm.Invert();
 
         AZ::Transform offsetTransform = AZ::Transform::CreateIdentity();
-        auto hasOffset = GetOffsetTransform(offsetTransform);
+        offsetTransform = GetOffsetTransform();
 
         AZStd::string frame{ msg.header.frame_id.c_str(), msg.header.frame_id.size() };
         AZStd::to_upper(frame.begin(), frame.end());
@@ -105,34 +104,21 @@ namespace SplineTools
             const auto& pose = poseStamped.pose;
             const AZ::Vector3 posePoint = AZ::Vector3(pose.position.x, pose.position.y, pose.position.z);
 
-            if (frame.empty())
-            {
-                if (hasOffset)
-                {
-                    auto referenceTranslation = offsetTransform.TransformPoint(posePoint);
-                    points[i] = worldTm.TransformPoint(referenceTranslation);
-                }
-                else
-                {
-                    points[i] = worldTm.TransformPoint(posePoint);
-                }
-            }
-            else if (frame == "LOCAL")
-            {
+            if (frame.empty()) {
+                auto referenceTranslation = offsetTransform.TransformPoint(posePoint);
+                points[i] = worldTm.TransformPoint(referenceTranslation);
+            } else if (frame == "LOCAL") {
                 points[i] = posePoint;
-            }
-            else if (frame == "WGS84" && m_config.m_allowWGS84)
-            {
+            } else if (frame == "WGS84" && m_config.m_allowWGS84) {
                 ROS2::WGS::WGS84Coordinate currentPositionWGS84;
                 currentPositionWGS84.m_latitude = pose.position.x;
                 currentPositionWGS84.m_longitude = pose.position.y;
                 currentPositionWGS84.m_altitude = pose.position.z;
-                AZ::Vector3 levelPosition{ 0 };
+                AZ::Vector3 levelPosition{0};
                 ROS2::GeoreferenceRequestsBus::BroadcastResult(
-                    levelPosition, &ROS2::GeoreferenceRequests::ConvertFromWGS84ToLevel, currentPositionWGS84);
+                        levelPosition, &ROS2::GeoreferenceRequests::ConvertFromWGS84ToLevel, currentPositionWGS84);
                 points[i] = worldTm.TransformPoint(levelPosition);
-            }
-            else
+            } else
             {
                 AZ_Error("SplineSubscriber", false, "Not implemented with frame %s", frame.data());
             }

diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h
@@ -21,7 +21,9 @@ namespace SplineTools
         void Deactivate() override;
 
     private:
-        bool GetOffsetTransform(AZ::Transform& transform);
+        //! Gets the offset transform for the entity, if offset is not available, returns identity transform
+        AZ::Transform GetOffsetTransform();
+
         void OnSplineReceived(const nav_msgs::msg::Path& msg);
         SplineSubscriberConfiguration m_config;
         rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr m_subscription;