Skip to content

[ROS2PoseControl] Add snap to ground with tag option #145

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: wc/pose_control
Choose a base branch
from
Open

[ROS2PoseControl] Add snap to ground with tag option #145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions Gems/ROS2PoseControl/Code/Include/ROS2PoseControl/ROS2PoseControlConfiguration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@ namespace ROS2PoseControl

AZ::Crc32 isUseTagOffset() const;

AZ::Crc32 isUseTagClamp() const;

AZ::Crc32 isUseClamp() const;

enum class TrackingMode
{
PoseMessages,
Expand All @@ -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;
};
Expand Down
17 changes: 15 additions & 2 deletions Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
AZStd::string query = m_configuration.m_clampTag;
const 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())
Expand Down
35 changes: 30 additions & 5 deletions Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControlConfiguration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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);

Expand Down Expand Up @@ -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,
Expand Down