diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp index b412094..3a8713a 100644 --- a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp +++ b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -54,6 +55,32 @@ namespace SplineTools }); } + bool SplineSubscriber::GetOffsetTransform(AZ::Transform& transform) + { + AZ::EBusAggregateResults 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 index 0dcfce4..7a1523d 100644 --- a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h +++ b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h @@ -2,6 +2,7 @@ #include "SplineSubscriberConfig.h" #include +#include #include #include #include @@ -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::SharedPtr m_subscription; diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriberConfig.cpp index 2cff0a7..cf85f71 100644 --- 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 index 8947cf1..49aa190 100644 --- 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