diff --git a/Gems/RobotecSplineTools/Code/Include/SplineTools/SplineToolsTypeIds.h b/Gems/RobotecSplineTools/Code/Include/SplineTools/SplineToolsTypeIds.h index 3a8eac19..a30a2aec 100644 --- a/Gems/RobotecSplineTools/Code/Include/SplineTools/SplineToolsTypeIds.h +++ b/Gems/RobotecSplineTools/Code/Include/SplineTools/SplineToolsTypeIds.h @@ -22,4 +22,6 @@ namespace SplineTools inline constexpr const char* SplineSubscriberComponentTypeId = "{89B8A92A-8F17-4C30-AE0D-6B088C133283}"; inline constexpr const char* SplineSubscriberConfigTypeId = "{44317FD2-51A1-41CA-BA44-F8BCAE9757CE}"; + inline constexpr const char* SplinePublisherComponentTypeId = "{29C02686-04F6-416D-8F47-D2456A3E114C}"; + inline constexpr const char* SplinePublisherConfigTypeId = "{DC7AC312-0F47-4EF2-A1B7-02E8716CF4EE}"; } // namespace SplineTools diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.cpp b/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.cpp new file mode 100644 index 00000000..226fd00a --- /dev/null +++ b/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.cpp @@ -0,0 +1,160 @@ +#include "SplinePublisher.h" + +#include +#include +#include + +namespace SplineTools +{ + void SplinePublisherConfiguration::Reflect(AZ::ReflectContext* context) + { + if (const auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(0) + ->Field("m_topicName", &SplinePublisherConfiguration::m_TopicConfig) + ->Field("m_updateFrequency", &SplinePublisherConfiguration::m_updateFrequency); + + if (const auto editContext = serializeContext->GetEditContext()) + { + editContext + ->Class( + "SplinePublisherConfiguration", "Configuration for the SplineSubscriber component") + ->ClassElement(AZ::Edit::ClassElements::Group, "SplineSubscriber Configuration") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &SplinePublisherConfiguration::m_updateFrequency, + "Update Frequency", + "How often path should be published (in ticks).") + ->Attribute(AZ::Edit::Attributes::Min, 0.0) + ->Attribute(AZ::Edit::Attributes::Step, 1.0) + ->DataElement( + AZ::Edit::UIHandlers::Default, &SplinePublisherConfiguration::m_TopicConfig, "Topic Config", "Topic Config"); + } + } + } + + void SplinePublisher::Reflect(AZ::ReflectContext* context) + { + SplinePublisherConfiguration::Reflect(context); + if (const auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class()->Version(0)->Field("m_config", &SplinePublisher::m_config); + + if (const auto editContext = serializeContext->GetEditContext()) + { + editContext->Class("SplinePathPublisher", "Enables to publish spline as a ROS 2 path.") + ->ClassElement(AZ::Edit::ClassElements::EditorData, "SplinePathPublisher") + ->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, + &SplinePublisher::m_config, + "Configuration", + "Configuration for the SplinePathPublisher component"); + } + } + } + + SplinePublisherConfiguration::SplinePublisherConfiguration() + { + m_TopicConfig.m_type = "nav_msgs::msg::Path"; + m_TopicConfig.m_topic = "spline"; + } + + SplinePublisher::SplinePublisher(SplinePublisherConfiguration config) + : m_config(std::move(config)) + { + } + + void SplinePublisher::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required) + { + required.push_back(AZ_CRC_CE("SplineService")); + required.push_back(AZ_CRC_CE("ROS2Frame")); + } + + void SplinePublisher::Activate() + { + m_ros2FramePtr = GetEntity()->FindComponent(); + if (!m_ros2FramePtr) + { + AZ_Warning("SplinePublisher::Activate", false, "ROS 2 frame component is not available!"); + return; + } + + // Format Ros Topic + if (!m_ros2FramePtr->GetNamespace().empty()) + { + m_config.m_TopicConfig.m_topic = + AZStd::string::format("%s/%s", m_ros2FramePtr->GetNamespace().c_str(), m_config.m_TopicConfig.m_topic.c_str()); + } + + // Create the ROS2 Publisher + if (const auto ros2Node = ROS2::ROS2Interface::Get()->GetNode()) + { + m_publisher = + ros2Node->create_publisher(m_config.m_TopicConfig.m_topic.c_str(), m_config.m_TopicConfig.GetQoS()); + + AZ::TickBus::Handler::BusConnect(); + } + } + + void SplinePublisher::Deactivate() + { + AZ::TickBus::Handler::BusDisconnect(); + m_publisher.reset(); + } + + void SplinePublisher::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) + { + if (m_frameNumber++ == m_config.m_updateFrequency) + { + PublishSplineAsPath(); + m_frameNumber = 0; + } + } + + void SplinePublisher::PublishSplineAsPath() const + { + if (!m_publisher) + { + return; + } + + AZ_Assert(m_ros2FramePtr, "ROS 2 frame component is not available!"); + + nav_msgs::msg::Path pathMessage; + pathMessage.header.frame_id = m_ros2FramePtr->GetFrameID().data(); + pathMessage.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp(); + + // Get Spline + AZStd::shared_ptr spline; + LmbrCentral::SplineComponentRequestBus::EventResult(spline, GetEntityId(), &LmbrCentral::SplineComponentRequests::GetSpline); + if (!spline) + { + AZ_Warning("SplinePublisher::PublishSplineAsPath", false, "Spline not found. Cannot generate spline path."); + return; + } + + // Get vertices from the spline + const size_t vertexCount = spline->GetVertexCount(); + pathMessage.poses.reserve(vertexCount); // Reserve known size + + for (size_t i = 0; i < vertexCount; ++i) + { + const AZ::Vector3& vertex = spline->GetVertex(i); + + // Use emplace_back to construct PoseStamped in place + pathMessage.poses.emplace_back(); + auto& poseStamped = pathMessage.poses.back(); + + // Set the pose values directly + poseStamped.pose.position.x = vertex.GetX(); + poseStamped.pose.position.y = vertex.GetY(); + poseStamped.pose.position.z = vertex.GetZ(); + } + + m_publisher->publish(pathMessage); + } +} // namespace SplineTools diff --git a/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.h b/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.h new file mode 100644 index 00000000..9a2946c2 --- /dev/null +++ b/Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace SplineTools +{ + struct SplinePublisherConfiguration + { + AZ_TYPE_INFO(SplinePublisherConfiguration, SplinePublisherConfigTypeId); + static void Reflect(AZ::ReflectContext* context); + + ROS2::TopicConfiguration m_TopicConfig{ rclcpp::ServicesQoS() }; + int m_updateFrequency = 10; + SplinePublisherConfiguration(); + }; + + class SplinePublisher final + : public AZ::Component + , protected AZ::TickBus::Handler + { + public: + AZ_COMPONENT(SplinePublisher, SplinePublisherComponentTypeId); + + static void Reflect(AZ::ReflectContext* context); + + SplinePublisher() = default; + ~SplinePublisher() override = default; + explicit SplinePublisher(SplinePublisherConfiguration config); + + static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + + // AZ::Component interface implementation + void Activate() override; + void Deactivate() override; + + protected: + // AZ::TickBus::Handler interface implementation + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + private: + void PublishSplineAsPath() const; + + SplinePublisherConfiguration m_config; + int m_frameNumber = 0; + rclcpp::Publisher::SharedPtr m_publisher; + ROS2::ROS2FrameComponent* m_ros2FramePtr = nullptr; + }; +} // namespace SplineTools diff --git a/Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp b/Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp index 00f1b85d..58c07a6c 100644 --- a/Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp +++ b/Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp @@ -1,9 +1,11 @@ #include "SplineToolsModuleInterface.h" + #include #include +#include #include #include #include @@ -24,7 +26,8 @@ namespace SplineTools m_descriptors.end(), { SplineToolsSystemComponent::CreateDescriptor(), VisualizeSplineComponent::CreateDescriptor(), - SplineSubscriber::CreateDescriptor() }); + SplineSubscriber::CreateDescriptor(), + SplinePublisher::CreateDescriptor() }); } AZ::ComponentTypeList SplineToolsModuleInterface::GetRequiredSystemComponents() const diff --git a/Gems/RobotecSplineTools/Code/splinetools_private_files.cmake b/Gems/RobotecSplineTools/Code/splinetools_private_files.cmake index 2c7ff46e..a1260bfc 100644 --- a/Gems/RobotecSplineTools/Code/splinetools_private_files.cmake +++ b/Gems/RobotecSplineTools/Code/splinetools_private_files.cmake @@ -1,13 +1,15 @@ set(FILES - Source/SplineToolsModuleInterface.cpp - Source/SplineToolsModuleInterface.h - Source/Clients/SplineToolsSystemComponent.cpp - Source/Clients/SplineToolsSystemComponent.h - Source/Clients/VisualizeSplineComponent.cpp - Source/Clients/VisualizeSplineComponent.h - Source/Clients/SplineSubscriber.h - Source/Clients/SplineSubscriber.cpp - Source/Clients/SplineSubscriberConfig.h - Source/Clients/SplineSubscriberConfig.cpp + Source/Clients/SplinePublisher.cpp + Source/Clients/SplinePublisher.h + Source/Clients/SplineSubscriber.cpp + Source/Clients/SplineSubscriber.h + Source/Clients/SplineSubscriberConfig.cpp + Source/Clients/SplineSubscriberConfig.h + Source/Clients/SplineToolsSystemComponent.cpp + Source/Clients/SplineToolsSystemComponent.h + Source/Clients/VisualizeSplineComponent.cpp + Source/Clients/VisualizeSplineComponent.h + Source/SplineToolsModuleInterface.cpp + Source/SplineToolsModuleInterface.h ) diff --git a/readme.md b/readme.md index 387456ee..c597984d 100644 --- a/readme.md +++ b/readme.md @@ -10,8 +10,10 @@ Toolset for joystick-controlled cameras and spline animation tools. # SplineTools The tools for expanding the usability of the Spline component in O3DE. It allows to: - - Import spline from CSV file - - Export spline to a CSV file +- Publish spline points as a path with ROS 2 +- Import spline from CSV file +- Export spline to a CSV file + Having a CSV file formatted as : ```csv x,y,z @@ -32,6 +34,9 @@ Add SplineToolsEditorComponent next to the [Spline component](https://docs.o3de. If you switch `Local Coordinates` to true, the component will interpret coordinates as local to entity origin. ![](doc/SplineToolsEditorComponent.png) +To publish spline path, add `SplinePublisher` next to the [Spline component](https://docs.o3de.org/docs/user-guide/components/reference/shape/spline/). +Adjust **update frequency** to set how often the path will be published. + ## Using geo-referenced data The CSV file can contain the following columns: `lat`, `lon`, `alt` where every row contains the WGS-84 coordinate of the spline's node.