Skip to content

Commit

Permalink
Small refactor
Browse files Browse the repository at this point in the history 
Signed-off-by: Michał Pełka <[email protected]>
  • Loading branch information
@michalpelka
michalpelka committed Nov 28, 2024
1 parent d7ee717 commit a0a6727
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 82 deletions.
142 changes: 61 additions & 81 deletions Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,142 +7,122 @@
#include <LmbrCentral/Shape/SplineComponentBus.h>
#include <ROS2/Georeference/GeoreferenceBus.h>

namespace SplineTools
{
namespace SplineTools {

void SplineSubscriber::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
{
void SplineSubscriber::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType &required) {
required.push_back(AZ_CRC_CE("TransformService"));
required.push_back(AZ_CRC_CE("SplineService"));
}

void SplineSubscriber::Reflect(AZ::ReflectContext* context)
{
void SplineSubscriber::Reflect(AZ::ReflectContext *context) {
SplineSubscriberConfiguration::Reflect(context);
if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
{
serializeContext->Class<SplineSubscriber, AZ::Component>()->Version(0)->Field("m_config", &SplineSubscriber::m_config);
if (auto editContext = serializeContext->GetEditContext())
{
editContext->Class<SplineSubscriber>("SplineSubscriber", "Configuration for the SplineSubscriber component")
->ClassElement(AZ::Edit::ClassElements::EditorData, "SplineSubscriber")
->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
->Attribute(AZ::Edit::Attributes::Category, "RobotecTools")
->Attribute(AZ::Edit::Attributes::AutoExpand, true)
->DataElement(
AZ::Edit::UIHandlers::Default,
&SplineSubscriber::m_config,
"Configuration",
"Configuration for the SplineSubscriber component");
if (auto serializeContext = azrtti_cast<AZ::SerializeContext *>(context)) {
serializeContext->Class<SplineSubscriber, AZ::Component>()->Version(0)->Field("m_config",
&SplineSubscriber::m_config);
if (auto editContext = serializeContext->GetEditContext()) {
editContext->Class<SplineSubscriber>("SplineSubscriber",
"Configuration for the SplineSubscriber component")
->ClassElement(AZ::Edit::ClassElements::EditorData, "SplineSubscriber")
->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
->Attribute(AZ::Edit::Attributes::Category, "RobotecTools")
->Attribute(AZ::Edit::Attributes::AutoExpand, true)
->DataElement(
AZ::Edit::UIHandlers::Default,
&SplineSubscriber::m_config,
"Configuration",
"Configuration for the SplineSubscriber component");
}
}
}

void SplineSubscriber::Activate()
{
if (m_config.m_resetOnActivation)
{
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(), &LmbrCentral::SplineComponentRequestBus::Events::ClearVertices);
void SplineSubscriber::Activate() {
if (m_config.m_resetOnActivation) {
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(),
&LmbrCentral::SplineComponentRequestBus::Events::ClearVertices);
}
auto node = ROS2::ROS2Interface::Get()->GetNode();
AZ_Assert(node, "ROS 2 Node is not available");
m_subscription = node->create_subscription<nav_msgs::msg::Path>(
m_config.m_topic.m_topic.data(),
m_config.m_topic.GetQoS(),
[this](const nav_msgs::msg::Path::SharedPtr msg)
{
OnSplineReceived(*msg);
});
m_config.m_topic.m_topic.data(),
m_config.m_topic.GetQoS(),
[this](const nav_msgs::msg::Path::SharedPtr msg) {
OnSplineReceived(*msg);
});
}

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);
LmbrCentral::TagGlobalRequestBus::EventResult(aggregator, tag, &LmbrCentral::TagGlobalRequests::RequestTaggedEntities);
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());
"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());
AZ_Warning("SplineSubscriber", !aggregator.values.empty(), "No entity with tag found %s.",
m_config.m_startOffsetTag.c_str())

if (!aggregator.values.empty())
{
if (!aggregator.values.empty()) {
AZ::Transform offsetTransform = AZ::Transform::CreateIdentity();
const AZ::EntityId& entityId = aggregator.values[0];
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)
{
void SplineSubscriber::OnSplineReceived(const nav_msgs::msg::Path &msg) {
AZ::SplinePtr splinePtr;
LmbrCentral::SplineComponentRequestBus::EventResult(
splinePtr, GetEntityId(), &LmbrCentral::SplineComponentRequestBus::Events::GetSpline);
splinePtr, GetEntityId(), &LmbrCentral::SplineComponentRequestBus::Events::GetSpline);
AZ_Assert(splinePtr, "SplineComponentRequestBus::Events::GetSpline failed");

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);
offsetTransform = GetOffsetTransform();

AZStd::string frame{ msg.header.frame_id.c_str(), msg.header.frame_id.size() };
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());
AZStd::vector<AZ::Vector3> points(msg.poses.size());
for (size_t i = 0; i < msg.poses.size(); ++i)
{
const auto& poseStamped = msg.poses[i];
const auto& pose = poseStamped.pose;
for (size_t i = 0; i < msg.poses.size(); ++i) {
const auto &poseStamped = msg.poses[i];
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());
}
}
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(), &LmbrCentral::SplineComponentRequestBus::Events::ClearVertices);
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(), &LmbrCentral::SplineComponentRequestBus::Events::SetVertices, points);
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(),
&LmbrCentral::SplineComponentRequestBus::Events::ClearVertices);
LmbrCentral::SplineComponentRequestBus::Event(GetEntityId(),
&LmbrCentral::SplineComponentRequestBus::Events::SetVertices,
points);
}

void SplineSubscriber::Deactivate()
{
void SplineSubscriber::Deactivate() {
m_subscription.reset();
}

Expand Down
4 changes: 3 additions & 1 deletion Gems/RobotecSplineTools/Code/Source/Clients/SplineSubscriber.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit a0a6727

Please sign in to comment.