Cherry pick from: Mp/publish joint state (#152)

* Added option to publish joint state from LUA

Signed-off-by: Michał Pełka <michal.pelka@robotec.ai>

* Make clang format happy

Signed-off-by: Michał Pełka <michal.pelka@robotec.ai>

* Code review

Signed-off-by: Jan Hanca <jan.hanca@robotec.ai>

---------

Signed-off-by: Michał Pełka <michal.pelka@robotec.ai>
Signed-off-by: Jan Hanca <jan.hanca@robotec.ai>
Co-authored-by: Jan Hanca <jan.hanca@robotec.ai>
(cherry picked from commit e850be3)
Signed-off-by: Piotr Jaroszek <piotr.jaroszek@robotec.ai>

Author: 2 people

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

@@ -12,6 +12,7 @@
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
 #include <sensor_msgs/msg/joy.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/empty.hpp>
@@ -208,6 +209,26 @@ 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

@@ -67,6 +67,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,15 @@ namespace ROS2ScriptIntegration
                         { "SteeringVelocity", "" },
                         { "Speed", "" },
                         { "Acceleration", "" },
-                        { "Jerk", "" } } });
+                        { "Jerk", "" } } })
+                ->Event(
+                    "PublishJointStateMsg",
+                    &PublisherRequestBus::Events::PublishJointStateMsg,
+                    { { { "Topic", "" },
+                        { "JointNames", "" },
+                        { "JointPositions", "" },
+                        { "JointVelocities", "" },
+                        { "JointEfforts", "" } } });
         }
     }
 } // 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
+ ```

Gems/ROS2ScriptIntegration/gem.json

@@ -1,6 +1,6 @@
 {
     "gem_name": "ROS2ScriptIntegration",
-    "version": "1.0.0",
+    "version": "2.1.0",
     "display_name": "ROS2ScriptIntegration",
     "license": "Apache-2.0",
     "license_url": "https://opensource.org/licenses/Apache-2.0",