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,133 @@
+#include "SplinePublisher.h"
+
+#include "ROS2/Utilities/ROS2Names.h"
+
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/ROS2Bus.h>
+
+namespace SplineTools
+{
+    SplinePublisherConfiguration::SplinePublisherConfiguration()
+    {
+        m_TopicConfig.m_type = "nav_msgs::msg::Path";
+        m_TopicConfig.m_topic = "spline_path";
+    }
+
+    void SplinePublisherConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<SplinePublisherConfiguration>()->Version(0)->Field(
+                "m_topicName", &SplinePublisherConfiguration::m_TopicConfig);
+            if (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_TopicConfig, "Topic Config", "Topic Config");
+            }
+        }
+    }
+
+    void SplinePublisher::Reflect(AZ::ReflectContext* context)
+    {
+        SplinePublisherConfiguration::Reflect(context);
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<SplinePublisher, AZ::Component>()->Version(0)->Field("m_config", &SplinePublisher::m_config);
+            if (auto editContext = serializeContext->GetEditContext())
+            {
+                editContext->Class<SplinePublisher>("SplinePathPublisher", "Enables to publish spline as a ros2 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");
+            }
+        }
+    }
+
+    SplinePublisher::SplinePublisher(const SplinePublisherConfiguration& config)
+        : m_config(config)
+    {
+    }
+
+    void SplinePublisher::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC("SplineService", 0x2b674d3c));
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+    }
+
+    void SplinePublisher::Activate()
+    {
+        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
+        if (ros2Node)
+        {
+            m_publisher =
+                ros2Node->create_publisher<nav_msgs::msg::Path>(m_config.m_TopicConfig.m_topic.data(), 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)
+    {
+        AZ_UNUSED(deltaTime);
+        AZ_UNUSED(time);
+
+        PublishSplineAsPath();
+    }
+
+    void SplinePublisher::PublishSplineAsPath() const
+    {
+        if (!m_publisher)
+        {
+            return;
+        }
+
+        const ROS2::ROS2FrameComponent* ros2Frame = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
+
+        nav_msgs::msg::Path pathMessage;
+        pathMessage.header.frame_id = ros2Frame->GetFrameID().data();
+        pathMessage.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+
+        // Retrieve spline data
+        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;
+        }
+
+        // Retrieve vertices from the spline
+        const size_t vertexCount = spline->GetVertexCount();
+        for (size_t i = 0; i < vertexCount; ++i)
+        {
+            const AZ::Vector3& vertex = spline->GetVertex(i);
+
+            // Convert each vertex into a PoseStamped and add to the path
+            geometry_msgs::msg::PoseStamped poseStamped;
+            poseStamped.pose.position.x = vertex.GetX();
+            poseStamped.pose.position.y = vertex.GetY();
+            poseStamped.pose.position.z = vertex.GetZ();
+            pathMessage.poses.push_back(poseStamped);
+        }
+
+        // Publish the message
+        m_publisher->publish(pathMessage);
+    }
+} // namespace SplineTools

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

@@ -0,0 +1,52 @@
+#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 <SplineTools/SplineToolsTypeIds.h>
+
+#include <nav_msgs/msg/path.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace SplineTools
+{
+    struct SplinePublisherConfiguration
+    {
+        SplinePublisherConfiguration();
+
+        AZ_TYPE_INFO(SplinePublisherConfiguration, SplinePublisherConfigTypeId);
+        static void Reflect(AZ::ReflectContext* context);
+        ROS2::TopicConfiguration m_TopicConfig{ rclcpp::ServicesQoS() };
+    };
+
+    class SplinePublisher
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+    {
+    public:
+        AZ_COMPONENT(SplinePublisher, SplinePublisherComponentTypeId);
+
+        static void Reflect(AZ::ReflectContext* context);
+
+        SplinePublisher() = default;
+        ~SplinePublisher() override = default;
+        explicit SplinePublisher(const SplinePublisherConfiguration& config);
+
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+
+        // AZ::Component interface implementation
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler interface implementation
+        void OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time) override;
+
+    private:
+        void PublishSplineAsPath() const;
+
+        SplinePublisherConfiguration m_config;
+        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr m_publisher;
+    };
+} // 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

@@ -10,4 +10,6 @@ set(FILES
     Source/Clients/SplineSubscriber.cpp
     Source/Clients/SplineSubscriberConfig.h
     Source/Clients/SplineSubscriberConfig.cpp
+    Source/Clients/SplinePublisher.h
+    Source/Clients/SplinePublisher.cpp
 )