diff --git a/Gems/ROS2PoseControl/Code/Include/ROS2PoseControl/ROS2PoseControlConfiguration.h b/Gems/ROS2PoseControl/Code/Include/ROS2PoseControl/ROS2PoseControlConfiguration.h index e1be9775..08ccb373 100644 --- a/Gems/ROS2PoseControl/Code/Include/ROS2PoseControl/ROS2PoseControlConfiguration.h +++ b/Gems/ROS2PoseControl/Code/Include/ROS2PoseControl/ROS2PoseControlConfiguration.h @@ -26,6 +26,10 @@ namespace ROS2PoseControl AZ::Crc32 isUseTagOffset() const; + AZ::Crc32 isUseTagClamp() const; + + AZ::Crc32 isUseClamp() const; + enum class TrackingMode { PoseMessages, @@ -42,9 +46,11 @@ namespace ROS2PoseControl bool m_clampToGround = false; float m_groundOffset = 0.0f; + bool m_useClampTag = false; + AZStd::string m_clampTag; - bool m_useTagOffset = false; - AZStd::string m_startOffsetTag; + bool m_useOffsetTag = false; + AZStd::string m_offsetTag; bool m_useWGS = false; }; diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp index 465c3ba9..39561e9f 100644 --- a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp +++ b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp @@ -366,9 +366,9 @@ namespace ROS2PoseControl AZ::Transform modifiedTransform = m_configuration.m_lockZAxis ? RemoveTilt(transform) : transform; - if (!m_configuration.m_startOffsetTag.empty()) + if (!m_configuration.m_offsetTag.empty()) { - auto startOffsetTransform = GetOffsetTransform(m_configuration.m_startOffsetTag); + auto startOffsetTransform = GetOffsetTransform(m_configuration.m_offsetTag); if (startOffsetTransform.has_value()) { modifiedTransform = startOffsetTransform.value() * modifiedTransform; @@ -419,6 +419,19 @@ namespace ROS2PoseControl request.m_direction = gravityDirection; request.m_distance = maxDistance; + if (m_configuration.m_useClampTag) + { + AZStd::string query = m_configuration.m_clampTag; + request.m_filterCallback = [&query](const AzPhysics::SimulatedBody* body, [[maybe_unused]] const Physics::Shape* shape) + { + const auto entityId = body->GetEntityId(); + LmbrCentral::Tags tags; + LmbrCentral::TagComponentRequestBus::EventResult(tags, entityId, &LmbrCentral::TagComponentRequests::GetTags); + return (tags.contains(AZ::Crc32(query))) ? AzPhysics::SceneQuery::QueryHitType::Block + : AzPhysics::SceneQuery::QueryHitType::None; + }; + } + AzPhysics::SceneQueryHits result = sceneInterface->QueryScene(sceneHandle, &request); if (!result.m_hits.empty()) diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControlConfiguration.cpp b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControlConfiguration.cpp index daea71f1..b812f281 100644 --- a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControlConfiguration.cpp +++ b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControlConfiguration.cpp @@ -22,7 +22,17 @@ namespace ROS2PoseControl AZ::Crc32 ROS2PoseControlConfiguration::isUseTagOffset() const { - return m_useTagOffset ? AZ::Edit::PropertyVisibility::Show : AZ::Edit::PropertyVisibility::Hide; + return m_useOffsetTag ? AZ::Edit::PropertyVisibility::Show : AZ::Edit::PropertyVisibility::Hide; + } + + AZ::Crc32 ROS2PoseControlConfiguration::isUseClamp() const + { + return m_clampToGround ? AZ::Edit::PropertyVisibility::Show : AZ::Edit::PropertyVisibility::Hide; + } + + AZ::Crc32 ROS2PoseControlConfiguration::isUseTagClamp() const + { + return m_useClampTag && m_clampToGround ? AZ::Edit::PropertyVisibility::Show : AZ::Edit::PropertyVisibility::Hide; } void ROS2PoseControlConfiguration::Reflect(AZ::ReflectContext* context) @@ -36,9 +46,11 @@ namespace ROS2PoseControl ->Field("m_targetFrame", &ROS2PoseControlConfiguration::m_targetFrame) ->Field("m_referenceFrame", &ROS2PoseControlConfiguration::m_referenceFrame) ->Field("lockZAxis", &ROS2PoseControlConfiguration::m_lockZAxis) - ->Field("useTagOffset", &ROS2PoseControlConfiguration::m_useTagOffset) - ->Field("startOffsetTag", &ROS2PoseControlConfiguration::m_startOffsetTag) + ->Field("useTagOffset", &ROS2PoseControlConfiguration::m_useOffsetTag) + ->Field("startOffsetTag", &ROS2PoseControlConfiguration::m_offsetTag) ->Field("m_clampToGround", &ROS2PoseControlConfiguration::m_clampToGround) + ->Field("useTagClamp", &ROS2PoseControlConfiguration::m_useClampTag) + ->Field("clampTag", &ROS2PoseControlConfiguration::m_clampTag) ->Field("m_groundOffset", &ROS2PoseControlConfiguration::m_groundOffset) ->Field("useWGS", &ROS2PoseControlConfiguration::m_useWGS); @@ -81,19 +93,32 @@ namespace ROS2PoseControl ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2PoseControlConfiguration::m_lockZAxis, "Lock Z Axis", "Lock Z axis") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2PoseControlConfiguration::m_useTagOffset, + &ROS2PoseControlConfiguration::m_useOffsetTag, "Use Tag Offset", "Use a tag that will be used to set the start offset for the entity.") ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree) ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2PoseControlConfiguration::m_startOffsetTag, + &ROS2PoseControlConfiguration::m_offsetTag, "Start Offset Tag", "Tag that will be used to set the start offset for the entity.") ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2PoseControlConfiguration::isUseTagOffset) ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2PoseControlConfiguration::m_clampToGround, "Clamp to Ground", "Clamp to ground") ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2PoseControlConfiguration::m_useClampTag, + "Use Tag Clamp", + "Use a tag that will be used to set the ground for the clamp for the entity.") + ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2PoseControlConfiguration::isUseClamp) + ->Attribute(AZ::Edit::Attributes::ChangeNotify, AZ::Edit::PropertyRefreshLevels::EntireTree) + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2PoseControlConfiguration::m_clampTag, + "Clamp Tag", + "Tag that will be used to clamp the entity to the ground.") + ->Attribute(AZ::Edit::Attributes::Visibility, &ROS2PoseControlConfiguration::isUseTagClamp) ->DataElement( AZ::Edit::UIHandlers::Default, &ROS2PoseControlConfiguration::m_groundOffset,