Skip to content

Commit b658ae0

Browse files
w-czerskiw-czerski
authored
Add Spline Path Publisher Component (#108)
--------- Signed-off-by: Wojciech Czerski <[email protected]>
1 parent d99ea5d commit b658ae0

File tree

6 files changed

+241
-13
lines changed

6 files changed

+241
-13
lines changed

‎Gems/RobotecSplineTools/Code/Include/SplineTools/SplineToolsTypeIds.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,4 +22,6 @@ namespace SplineTools
2222
inline constexpr const char* SplineSubscriberComponentTypeId = "{89B8A92A-8F17-4C30-AE0D-6B088C133283}";
2323
inline constexpr const char* SplineSubscriberConfigTypeId = "{44317FD2-51A1-41CA-BA44-F8BCAE9757CE}";
2424

25+
inline constexpr const char* SplinePublisherComponentTypeId = "{29C02686-04F6-416D-8F47-D2456A3E114C}";
26+
inline constexpr const char* SplinePublisherConfigTypeId = "{DC7AC312-0F47-4EF2-A1B7-02E8716CF4EE}";
2527
} // namespace SplineTools

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

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
#include "SplinePublisher.h"
2+
3+
#include <ROS2/ROS2Bus.h>
4+
#include <ROS2/Utilities/ROS2Names.h>
5+
#include <utility>
6+
7+
namespace SplineTools
8+
{
9+
void SplinePublisherConfiguration::Reflect(AZ::ReflectContext* context)
10+
{
11+
if (const auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
12+
{
13+
serializeContext->Class<SplinePublisherConfiguration>()
14+
->Version(0)
15+
->Field("m_topicName", &SplinePublisherConfiguration::m_TopicConfig)
16+
->Field("m_updateFrequency", &SplinePublisherConfiguration::m_updateFrequency);
17+
18+
if (const auto editContext = serializeContext->GetEditContext())
19+
{
20+
editContext
21+
->Class<SplinePublisherConfiguration>(
22+
"SplinePublisherConfiguration", "Configuration for the SplineSubscriber component")
23+
->ClassElement(AZ::Edit::ClassElements::Group, "SplineSubscriber Configuration")
24+
->DataElement(
25+
AZ::Edit::UIHandlers::Default,
26+
&SplinePublisherConfiguration::m_updateFrequency,
27+
"Update Frequency",
28+
"How often path should be published (in ticks).")
29+
->Attribute(AZ::Edit::Attributes::Min, 0.0)
30+
->Attribute(AZ::Edit::Attributes::Step, 1.0)
31+
->DataElement(
32+
AZ::Edit::UIHandlers::Default, &SplinePublisherConfiguration::m_TopicConfig, "Topic Config", "Topic Config");
33+
}
34+
}
35+
}
36+
37+
void SplinePublisher::Reflect(AZ::ReflectContext* context)
38+
{
39+
SplinePublisherConfiguration::Reflect(context);
40+
if (const auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
41+
{
42+
serializeContext->Class<SplinePublisher, AZ::Component>()->Version(0)->Field("m_config", &SplinePublisher::m_config);
43+
44+
if (const auto editContext = serializeContext->GetEditContext())
45+
{
46+
editContext->Class<SplinePublisher>("SplinePathPublisher", "Enables to publish spline as a ROS 2 path.")
47+
->ClassElement(AZ::Edit::ClassElements::EditorData, "SplinePathPublisher")
48+
->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
49+
->Attribute(AZ::Edit::Attributes::Category, "RobotecTools")
50+
->Attribute(AZ::Edit::Attributes::AutoExpand, true)
51+
->DataElement(
52+
AZ::Edit::UIHandlers::Default,
53+
&SplinePublisher::m_config,
54+
"Configuration",
55+
"Configuration for the SplinePathPublisher component");
56+
}
57+
}
58+
}
59+
60+
SplinePublisherConfiguration::SplinePublisherConfiguration()
61+
{
62+
m_TopicConfig.m_type = "nav_msgs::msg::Path";
63+
m_TopicConfig.m_topic = "spline";
64+
}
65+
66+
SplinePublisher::SplinePublisher(SplinePublisherConfiguration config)
67+
: m_config(std::move(config))
68+
{
69+
}
70+
71+
void SplinePublisher::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
72+
{
73+
required.push_back(AZ_CRC_CE("SplineService"));
74+
required.push_back(AZ_CRC_CE("ROS2Frame"));
75+
}
76+
77+
void SplinePublisher::Activate()
78+
{
79+
m_ros2FramePtr = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
80+
if (!m_ros2FramePtr)
81+
{
82+
AZ_Warning("SplinePublisher::Activate", false, "ROS 2 frame component is not available!");
83+
return;
84+
}
85+
86+
// Format Ros Topic
87+
if (!m_ros2FramePtr->GetNamespace().empty())
88+
{
89+
m_config.m_TopicConfig.m_topic =
90+
AZStd::string::format("%s/%s", m_ros2FramePtr->GetNamespace().c_str(), m_config.m_TopicConfig.m_topic.c_str());
91+
}
92+
93+
// Create the ROS2 Publisher
94+
if (const auto ros2Node = ROS2::ROS2Interface::Get()->GetNode())
95+
{
96+
m_publisher =
97+
ros2Node->create_publisher<nav_msgs::msg::Path>(m_config.m_TopicConfig.m_topic.c_str(), m_config.m_TopicConfig.GetQoS());
98+
99+
AZ::TickBus::Handler::BusConnect();
100+
}
101+
}
102+
103+
void SplinePublisher::Deactivate()
104+
{
105+
AZ::TickBus::Handler::BusDisconnect();
106+
m_publisher.reset();
107+
}
108+
109+
void SplinePublisher::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
110+
{
111+
if (m_frameNumber++ == m_config.m_updateFrequency)
112+
{
113+
PublishSplineAsPath();
114+
m_frameNumber = 0;
115+
}
116+
}
117+
118+
void SplinePublisher::PublishSplineAsPath() const
119+
{
120+
if (!m_publisher)
121+
{
122+
return;
123+
}
124+
125+
AZ_Assert(m_ros2FramePtr, "ROS 2 frame component is not available!");
126+
127+
nav_msgs::msg::Path pathMessage;
128+
pathMessage.header.frame_id = m_ros2FramePtr->GetFrameID().data();
129+
pathMessage.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
130+
131+
// Get Spline
132+
AZStd::shared_ptr<AZ::Spline> spline;
133+
LmbrCentral::SplineComponentRequestBus::EventResult(spline, GetEntityId(), &LmbrCentral::SplineComponentRequests::GetSpline);
134+
if (!spline)
135+
{
136+
AZ_Warning("SplinePublisher::PublishSplineAsPath", false, "Spline not found. Cannot generate spline path.");
137+
return;
138+
}
139+
140+
// Get vertices from the spline
141+
const size_t vertexCount = spline->GetVertexCount();
142+
pathMessage.poses.reserve(vertexCount); // Reserve known size
143+
144+
for (size_t i = 0; i < vertexCount; ++i)
145+
{
146+
const AZ::Vector3& vertex = spline->GetVertex(i);
147+
148+
// Use emplace_back to construct PoseStamped in place
149+
pathMessage.poses.emplace_back();
150+
auto& poseStamped = pathMessage.poses.back();
151+
152+
// Set the pose values directly
153+
poseStamped.pose.position.x = vertex.GetX();
154+
poseStamped.pose.position.y = vertex.GetY();
155+
poseStamped.pose.position.z = vertex.GetZ();
156+
}
157+
158+
m_publisher->publish(pathMessage);
159+
}
160+
} // namespace SplineTools

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

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
#pragma once
2+
3+
#include <AzCore/Component/Component.h>
4+
#include <AzCore/Serialization/SerializeContext.h>
5+
#include <AzFramework/Components/TransformComponent.h>
6+
#include <LmbrCentral/Shape/SplineComponentBus.h>
7+
#include <ROS2/Communication/TopicConfiguration.h>
8+
#include <ROS2/Frame/ROS2FrameComponent.h>
9+
#include <SplineTools/SplineToolsTypeIds.h>
10+
#include <nav_msgs/msg/path.hpp>
11+
#include <rclcpp/rclcpp.hpp>
12+
13+
namespace SplineTools
14+
{
15+
struct SplinePublisherConfiguration
16+
{
17+
AZ_TYPE_INFO(SplinePublisherConfiguration, SplinePublisherConfigTypeId);
18+
static void Reflect(AZ::ReflectContext* context);
19+
20+
ROS2::TopicConfiguration m_TopicConfig{ rclcpp::ServicesQoS() };
21+
int m_updateFrequency = 10;
22+
SplinePublisherConfiguration();
23+
};
24+
25+
class SplinePublisher final
26+
: public AZ::Component
27+
, protected AZ::TickBus::Handler
28+
{
29+
public:
30+
AZ_COMPONENT(SplinePublisher, SplinePublisherComponentTypeId);
31+
32+
static void Reflect(AZ::ReflectContext* context);
33+
34+
SplinePublisher() = default;
35+
~SplinePublisher() override = default;
36+
explicit SplinePublisher(SplinePublisherConfiguration config);
37+
38+
static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
39+
40+
// AZ::Component interface implementation
41+
void Activate() override;
42+
void Deactivate() override;
43+
44+
protected:
45+
// AZ::TickBus::Handler interface implementation
46+
void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
47+
48+
private:
49+
void PublishSplineAsPath() const;
50+
51+
SplinePublisherConfiguration m_config;
52+
int m_frameNumber = 0;
53+
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr m_publisher;
54+
ROS2::ROS2FrameComponent* m_ros2FramePtr = nullptr;
55+
};
56+
} // namespace SplineTools

‎Gems/RobotecSplineTools/Code/Source/SplineToolsModuleInterface.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,11 @@
11

22
#include "SplineToolsModuleInterface.h"
3+
34
#include <AzCore/Memory/Memory.h>
45

56
#include <SplineTools/SplineToolsTypeIds.h>
67

8+
#include <Clients/SplinePublisher.h>
79
#include <Clients/SplineSubscriber.h>
810
#include <Clients/SplineToolsSystemComponent.h>
911
#include <Clients/VisualizeSplineComponent.h>
@@ -24,7 +26,8 @@ namespace SplineTools
2426
m_descriptors.end(),
2527
{ SplineToolsSystemComponent::CreateDescriptor(),
2628
VisualizeSplineComponent::CreateDescriptor(),
27-
SplineSubscriber::CreateDescriptor() });
29+
SplineSubscriber::CreateDescriptor(),
30+
SplinePublisher::CreateDescriptor() });
2831
}
2932

3033
AZ::ComponentTypeList SplineToolsModuleInterface::GetRequiredSystemComponents() const

‎Gems/RobotecSplineTools/Code/splinetools_private_files.cmake‎

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,15 @@
11

22
set(FILES
3-
Source/SplineToolsModuleInterface.cpp
4-
Source/SplineToolsModuleInterface.h
5-
Source/Clients/SplineToolsSystemComponent.cpp
6-
Source/Clients/SplineToolsSystemComponent.h
7-
Source/Clients/VisualizeSplineComponent.cpp
8-
Source/Clients/VisualizeSplineComponent.h
9-
Source/Clients/SplineSubscriber.h
10-
Source/Clients/SplineSubscriber.cpp
11-
Source/Clients/SplineSubscriberConfig.h
12-
Source/Clients/SplineSubscriberConfig.cpp
3+
Source/Clients/SplinePublisher.cpp
4+
Source/Clients/SplinePublisher.h
5+
Source/Clients/SplineSubscriber.cpp
6+
Source/Clients/SplineSubscriber.h
7+
Source/Clients/SplineSubscriberConfig.cpp
8+
Source/Clients/SplineSubscriberConfig.h
9+
Source/Clients/SplineToolsSystemComponent.cpp
10+
Source/Clients/SplineToolsSystemComponent.h
11+
Source/Clients/VisualizeSplineComponent.cpp
12+
Source/Clients/VisualizeSplineComponent.h
13+
Source/SplineToolsModuleInterface.cpp
14+
Source/SplineToolsModuleInterface.h
1315
)

‎readme.md‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,10 @@ Toolset for joystick-controlled cameras and spline animation tools.
1010
# SplineTools
1111
The tools for expanding the usability of the Spline component in O3DE.
1212
It allows to:
13-
- Import spline from CSV file
14-
- Export spline to a CSV file
13+
- Publish spline points as a path with ROS 2
14+
- Import spline from CSV file
15+
- Export spline to a CSV file
16+
1517
Having a CSV file formatted as :
1618
```csv
1719
x,y,z
@@ -32,6 +34,9 @@ Add SplineToolsEditorComponent next to the [Spline component](https://docs.o3de.
3234
If you switch `Local Coordinates` to true, the component will interpret coordinates as local to entity origin.
3335
![](doc/SplineToolsEditorComponent.png)
3436

37+
To publish spline path, add `SplinePublisher` next to the [Spline component](https://docs.o3de.org/docs/user-guide/components/reference/shape/spline/).
38+
Adjust **update frequency** to set how often the path will be published.
39+
3540
## Using geo-referenced data
3641

3742
The CSV file can contain the following columns: `lat`, `lon`, `alt` where every row contains the WGS-84 coordinate of the spline's node.

0 commit comments

Comments
 (0)