Add Spline Path Publisher Component

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

Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.cpp

@@ -0,0 +1,160 @@
+#include "SplinePublisher.h"
+
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <utility>
+
+namespace SplineTools
+{
+    void SplinePublisherConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (const auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<SplinePublisherConfiguration>()
+                ->Version(0)
+                ->Field("m_topicName", &SplinePublisherConfiguration::m_TopicConfig)
+                ->Field("m_updateFrequency", &SplinePublisherConfiguration::m_updateFrequency);
+
+            if (const auto editContext = serializeContext->GetEditContext())
+            {
+                editContext
+                    ->Class<SplinePublisherConfiguration>(
+                        "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<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<SplinePublisher, AZ::Component>()->Version(0)->Field("m_config", &SplinePublisher::m_config);
+
+            if (const auto editContext = serializeContext->GetEditContext())
+            {
+                editContext->Class<SplinePublisher>("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<ROS2::ROS2FrameComponent>();
+        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<nav_msgs::msg::Path>(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<AZ::Spline> 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

Gems/RobotecSplineTools/Code/Source/Clients/SplinePublisher.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <LmbrCentral/Shape/SplineComponentBus.h>
+#include <ROS2/Communication/TopicConfiguration.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <SplineTools/SplineToolsTypeIds.h>
+#include <nav_msgs/msg/path.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+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<nav_msgs::msg::Path>::SharedPtr m_publisher;
+        ROS2::ROS2FrameComponent* m_ros2FramePtr = nullptr;
+    };
+} // namespace SplineTools

Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp

@@ -1,9 +1,11 @@
 
 #include "SplineToolsModuleInterface.h"
+
 #include <AzCore/Memory/Memory.h>
 
 #include <SplineTools/SplineToolsTypeIds.h>
 
+#include <Clients/SplinePublisher.h>
 #include <Clients/SplineSubscriber.h>
 #include <Clients/SplineToolsSystemComponent.h>
 #include <Clients/VisualizeSplineComponent.h>
@@ -24,7 +26,8 @@ namespace SplineTools
             m_descriptors.end(),
             { SplineToolsSystemComponent::CreateDescriptor(),
               VisualizeSplineComponent::CreateDescriptor(),
-              SplineSubscriber::CreateDescriptor() });
+              SplineSubscriber::CreateDescriptor(),
+              SplinePublisher::CreateDescriptor() });
     }
 
     AZ::ComponentTypeList SplineToolsModuleInterface::GetRequiredSystemComponents() const

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
 )

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.