Broadcast transforms en masse (#584)

Signed-off-by: Adam Dabrowski <[email protected]>

Gems/ROS2/Code/Include/ROS2/ROS2Bus.h

@@ -50,7 +50,7 @@ namespace ROS2
         //! only once and are to be used when the spatial relationship between two frames does not change.
         //! @note Transforms are already published by each ROS2FrameComponent.
         //! Use this function directly only when default behavior of ROS2FrameComponent is not sufficient.
-        virtual void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const = 0;
+        virtual void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) = 0;
 
         //! Obtains a simulation clock that is used across simulation.
         //! @returns constant reference to currently running clock.

Gems/ROS2/Code/Source/ROS2SystemComponent.cpp

@@ -182,11 +182,11 @@ namespace ROS2
         return *m_simulationClock;
     }
 
-    void ROS2SystemComponent::BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const
+    void ROS2SystemComponent::BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic)
     {
         if (isDynamic)
         {
-            m_dynamicTFBroadcaster->sendTransform(t);
+            m_frameTransforms.push_back(t);
         }
         else
         {
@@ -198,6 +198,9 @@ namespace ROS2
     {
         if (rclcpp::ok())
         {
+            m_dynamicTFBroadcaster->sendTransform(m_frameTransforms);
+            m_frameTransforms.clear();
+
             m_simulationClock->Tick();
             m_executor->spin_some();
         }

Gems/ROS2/Code/Source/ROS2SystemComponent.h

@@ -62,7 +62,7 @@ namespace ROS2
         // ROS2RequestBus::Handler overrides
         std::shared_ptr<rclcpp::Node> GetNode() const override;
         builtin_interfaces::msg::Time GetROSTimestamp() const override;
-        void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) const override;
+        void BroadcastTransform(const geometry_msgs::msg::TransformStamped& t, bool isDynamic) override;
         const SimulationClock& GetSimulationClock() const override;
         //////////////////////////////////////////////////////////////////////////
 
@@ -83,6 +83,8 @@ namespace ROS2
     private:
         void InitClock();
 
+        std::vector<geometry_msgs::msg::TransformStamped> m_frameTransforms;
+
         std::shared_ptr<rclcpp::Node> m_ros2Node;
         AZStd::shared_ptr<rclcpp::executors::SingleThreadedExecutor> m_executor;
         AZStd::unique_ptr<tf2_ros::TransformBroadcaster> m_dynamicTFBroadcaster;