Added option to publish joint state from LUA

Signed-off-by: Michał Pełka <[email protected]>

Author: michalpelka

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<AZStd::string>& names,
+            const AZStd::vector<float>& positions,
+            const AZStd::vector<float>& velocities,
+            const AZStd::vector<float>& efforts) = 0;
         static void Reflect(AZ::ReflectContext* context);
     };
 

Gems/ROS2ScriptIntegration/Code/Source/Clients/PublisherSystemComponent.cpp

@@ -8,6 +8,7 @@
 #include <AzCore/RTTI/BehaviorContext.h>
 #include <AzCore/Serialization/SerializeContext.h>
 
+#include <sensor_msgs/msg/joint_state.hpp>
 #include <ackermann_msgs/msg/ackermann_drive.hpp>
 #include <geometry_msgs/msg/point32.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -211,6 +212,27 @@ namespace ROS2ScriptIntegration
         PublishMessage(topicName, message);
     }
 
+    void PublisherSystemComponent::PublishJointStateMsg(
+        const AZStd::string& topicName,
+        const AZStd::vector<AZStd::string>& names,
+        const AZStd::vector<float>& positions,
+        const AZStd::vector<float>& velocities,
+        const AZStd::vector<float>& 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<double>(positions.begin(), positions.end());
+        message.velocity = std::vector<double>(velocities.begin(), velocities.end());
+        message.effort = std::vector<double>(efforts.begin(), efforts.end());
+        PublishMessage(topicName, message);
+    }
+
+
     template<typename MessageType>
     std::shared_ptr<rclcpp::Publisher<MessageType>> PublisherSystemComponent::GetOrCreatePublisher(const AZStd::string& topicName)
     {

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<AZStd::string>& names,
+            const AZStd::vector<float>& positions,
+            const AZStd::vector<float>& velocities,
+            const AZStd::vector<float>& efforts) override;
+
         // AZ::Component overrides ...
         void Init() override;
 

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

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
+ ```