From 82093178ded518b4b27774f0d4668502a6e6a3d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 7 Oct 2025 17:07:49 +0200 Subject: [PATCH 1/2] Added option to publish joint state from LUA MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../ROS2ScriptPublisherBus.h | 7 ++- .../Clients/PublisherSystemComponent.cpp | 22 +++++++++ .../Source/Clients/PublisherSystemComponent.h | 7 +++ .../Source/Clients/ROS2ScriptPublisherBus.cpp | 5 ++- Gems/ROS2ScriptIntegration/README.md | 45 +++++++++++++++++++ 5 files changed, 84 insertions(+), 2 deletions(-) diff --git a/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h b/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h index 9850e589..2e925a0f 100644 --- a/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h +++ b/Gems/ROS2ScriptIntegration/Code/Include/ROS2ScriptIntegration/ROS2ScriptPublisherBus.h @@ -40,7 +40,12 @@ namespace ROS2ScriptIntegration const AZStd::string& topicName, const AZStd::string& frame, const AZ::Transform& transform) = 0; virtual void PublishAckermannDriveMsg( const AZStd::string& topicName, float steeringAngle, float steeringVelocity, float speed, float acceleration, float jerk) = 0; - + virtual void PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) = 0; static void Reflect(AZ::ReflectContext* context); }; diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp index 2c92c893..8c1df70c 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -211,6 +212,27 @@ namespace ROS2ScriptIntegration PublishMessage(topicName, message); } + void PublisherSystemComponent::PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) + { + sensor_msgs::msg::JointState message; + message.header.stamp = ROS2::ROS2ClockInterface::Get()->GetROSTimestamp(); + message.name.resize(names.size()); + for (size_t i = 0; i < names.size(); ++i) + { + message.name[i] = std::string(names[i].c_str()); + } + message.position = std::vector(positions.begin(), positions.end()); + message.velocity = std::vector(velocities.begin(), velocities.end()); + message.effort = std::vector(efforts.begin(), efforts.end()); + PublishMessage(topicName, message); + } + + template std::shared_ptr> PublisherSystemComponent::GetOrCreatePublisher(const AZStd::string& topicName) { diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h index 4fc69f9b..ff6748c7 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.h @@ -66,6 +66,13 @@ namespace ROS2ScriptIntegration const AZStd::string& topicName, float steeringAngle, float steeringVelocity, float speed, float acceleration, float jerk) override; + void PublishJointStateMsg( + const AZStd::string& topicName, + const AZStd::vector& names, + const AZStd::vector& positions, + const AZStd::vector& velocities, + const AZStd::vector& efforts) override; + // AZ::Component overrides ... void Init() override; diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp index 4889454b..07718952 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp @@ -53,7 +53,10 @@ namespace ROS2ScriptIntegration { "SteeringVelocity", "" }, { "Speed", "" }, { "Acceleration", "" }, - { "Jerk", "" } } }); + { "Jerk", "" } } }) + ->Event ( + "PublishJointStateMsg", + &PublisherRequestBus::Events::PublishJointStateMsg); } } } // namespace ROS2ScriptIntegration diff --git a/Gems/ROS2ScriptIntegration/README.md b/Gems/ROS2ScriptIntegration/README.md index 4438e7f9..62bcd686 100644 --- a/Gems/ROS2ScriptIntegration/README.md +++ b/Gems/ROS2ScriptIntegration/README.md @@ -9,6 +9,7 @@ Currently, the supported message types are limited to the following: - a subset of geometry messages - a subset of standard messages - joystick message +- joint state message ## Hello world publisher @@ -80,3 +81,47 @@ function myscript:OnStdMsgString(message) Debug.Log("I heard : " .. message) end ``` + +### Using messages with arrays (e.g. sensor_msgs/JointState) + +- you need to declare `vector_*` types to create C++ array equivalents +- you need to use `push_back` method to add elements to the array +- those arrays can be passed to the publisher method +Example: + +```lua +local LogJoints = +{ + Properties = {} +} + +function LogJoints:OnActivate() + self.tickBusHandler = TickBus.Connect(self) +end + +function LogJoints:OnDeactivate() + self.tickBusHandler:Disconnect() +end + +function LogJoints:OnTick(deltaTime, timePoint) + names = vector_basic_string_char_char_traits_char() + names:push_back("egoback_left_wheel_joint") + names:push_back("egoback_right_wheel_joint") + + joints = vector_float() + joints:push_back(0) + joints:push_back(0) + + velocities = vector_float() + velocities:push_back(0) + velocities:push_back(0) + + efforts = vector_float() + efforts:push_back(0) + efforts:push_back(0) + + PublisherRequestBus.Broadcast.PublishJointStateMsg("joint_states",names,joints,velocities,efforts) +end + +return LogJoints + ``` \ No newline at end of file From 7266e9c89aba3b84bdd8f122d76db7d69703a55c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Tue, 7 Oct 2025 17:10:05 +0200 Subject: [PATCH 2/2] Make clang format happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../Code/Source/Clients/PublisherSystemComponent.cpp | 3 +-- .../Code/Source/Clients/ROS2ScriptPublisherBus.cpp | 4 +--- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp index 8c1df70c..4422a46f 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -16,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -232,7 +232,6 @@ namespace ROS2ScriptIntegration PublishMessage(topicName, message); } - template std::shared_ptr> PublisherSystemComponent::GetOrCreatePublisher(const AZStd::string& topicName) { diff --git a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp index 07718952..5c53e233 100644 --- a/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp +++ b/Gems/ROS2ScriptIntegration/Code/Source/Clients/ROS2ScriptPublisherBus.cpp @@ -54,9 +54,7 @@ namespace ROS2ScriptIntegration { "Speed", "" }, { "Acceleration", "" }, { "Jerk", "" } } }) - ->Event ( - "PublishJointStateMsg", - &PublisherRequestBus::Events::PublishJointStateMsg); + ->Event("PublishJointStateMsg", &PublisherRequestBus::Events::PublishJointStateMsg); } } } // namespace ROS2ScriptIntegration