Increase max publishing frequency of joint_states messages (#412)

Increased the max publishing frequency using 1 bus request per joint

---------

Signed-off-by: Antoni Puch <[email protected]>

Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponent.h

@@ -18,6 +18,7 @@ namespace ROS2
     //! Captures common behavior of ROS2 sensor Components.
     //! Sensors acquire data from the simulation engine and publish it to ROS2 ecosystem.
     //! Derive this Component to implement a new ROS2 sensor. Each sensor Component requires ROS2FrameComponent.
+    //! For high frequency sensors also derive PhysicsCallbackHandler.
     class ROS2SensorComponent
         : public AZ::Component
         , public AZ::TickBus::Handler

Gems/ROS2/Code/Source/Utilities/PhysicsCallbackHandler.h → Gems/ROS2/Code/Include/ROS2/Utilities/PhysicsCallbackHandler.h

@@ -13,31 +13,26 @@
 
 namespace ROS2::Utils
 {
-    //! Helper class that register OnSceneSimulationFinishHandler and retrieve handle to Simulated Body from the Default Scene.
+    //! A class that registers physics tick callbacks from the Default Scene.
     class PhysicsCallbackHandler
     {
     protected:
-        //! Install to default physics scene callbacks, when the scene is created, the @member m_bodyHandle is populated with the handle to
-        //! simulated body.
-        //! @param m_entityId Entity id to get m_bodyHandle to.
-        void InstallPhysicalCallback(AZ::EntityId m_entityId);
+        //! Install default physics scene callbacks
+        void InstallPhysicalCallback();
 
         //! Removes all attached callbacks
         void RemovePhysicalCallback();
 
         //! Called multiple times per frame after every inner loop of physics engine.
-        //! It virtual version of  callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
+        //! It's a virtual version of callback AzPhysics::SceneEvents::OnSceneSimulationFinishHandler.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
         //! @param deltaTime - update of simulated time in seconds.
-        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) {};
+        virtual void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime){};
 
-        //! Callback called on begging of the first physical simulation.
-        //! inner loop of physics engine.
+        //! Callback called on beginning of the first physical simulation inner loop of physics engine.
         //! @param sceneHandle - scene handle, only handle to Default Scene is expected
-        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) {};
+        virtual void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle){};
 
-        //! Handler to simulated physical body
-        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     private:
         AzPhysics::SceneEvents::OnSceneSimulationFinishHandler m_onSceneSimulationEvent;
         AzPhysics::SceneEvents::OnSceneSimulationStartHandler m_onSceneSimulationStart;

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -73,13 +73,23 @@ namespace ROS2
 
     void ROS2ImuSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
+        required.push_back(AZ_CRC_CE("PhysicsDynamicRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
     }
 
     void ROS2ImuSensorComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
+    }
+
+    void ROS2ImuSensorComponent::OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle)
+    {
+        AzPhysics::RigidBody* rigidBody = nullptr;
+        AZ::EntityId entityId = GetEntityId();
+        Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(rigidBody, "Entity %s does not have rigid body.", entityId.ToString().c_str());
+
+        m_bodyHandle = rigidBody->m_bodyHandle;
     }
 
     void ROS2ImuSensorComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime)

Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h

@@ -13,7 +13,7 @@
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 
@@ -61,8 +61,12 @@ namespace ROS2
         void SetupRefreshLoop() override;
 
         // ROS2::Utils::PhysicsCallbackHandler overrides ...
+        void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) override;
         void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
 
         AZ::Matrix3x3 ToDiagonalCovarianceMatrix(const AZ::Vector3& variance);
+
+        // Handle to the simulated physical body
+        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp

@@ -7,6 +7,7 @@
  */
 
 #include "JointStatePublisher.h"
+#include "ManipulationUtils.h"
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Names.h>
@@ -30,36 +31,42 @@ namespace ROS2
         rosHeader.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
         m_jointStateMsg.header = rosHeader;
 
+        AZ_Assert(m_jointNames.size() == m_jointStateMsg.name.size(), "The expected message size doesn't match with the joint list size");
+
+        for (size_t i = 0; i < m_jointStateMsg.name.size(); i++)
+        {
+            m_jointStateMsg.name[i] = m_jointNames[i].c_str();
+            JointInfo& jointInfo = m_jointInfos[i];
+
+            auto jointStateData = Utils::GetJointState(jointInfo);
+
+            m_jointStateMsg.position[i] = jointStateData.position;
+            m_jointStateMsg.velocity[i] = jointStateData.velocity;
+            m_jointStateMsg.effort[i] = jointStateData.effort;
+        }
+        m_jointStatePublisher->publish(m_jointStateMsg);
+    }
+
+    void JointStatePublisher::InitializePublisher(AZ::EntityId entityId)
+    {
         ManipulationJoints manipulatorJoints;
         JointsManipulationRequestBus::EventResult(manipulatorJoints, m_context.m_entityId, &JointsManipulationRequests::GetJoints);
 
+        for (const auto& [jointName, jointInfo] : manipulatorJoints)
+        {
+            m_jointNames.push_back(jointName);
+            m_jointInfos.push_back(jointInfo);
+        }
+
         m_jointStateMsg.name.resize(manipulatorJoints.size());
         m_jointStateMsg.position.resize(manipulatorJoints.size());
         m_jointStateMsg.velocity.resize(manipulatorJoints.size());
         m_jointStateMsg.effort.resize(manipulatorJoints.size());
-        size_t i = 0;
-        for (const auto& [jointName, jointInfo] : manipulatorJoints)
-        {
-            AZ::Outcome<float, AZStd::string> result;
-            JointsManipulationRequestBus::EventResult(
-                result, m_context.m_entityId, &JointsManipulationRequests::GetJointPosition, jointName);
-            auto currentJointPosition = result.GetValue();
-            JointsManipulationRequestBus::EventResult(
-                result, m_context.m_entityId, &JointsManipulationRequests::GetJointVelocity, jointName);
-            auto currentJointVelocity = result.GetValue();
-            JointsManipulationRequestBus::EventResult(result, m_context.m_entityId, &JointsManipulationRequests::GetJointEffort, jointName);
-            auto currentJointEffort = result.GetValue();
 
-            m_jointStateMsg.name[i] = jointName.c_str();
-            m_jointStateMsg.position[i] = currentJointPosition;
-            m_jointStateMsg.velocity[i] = currentJointVelocity;
-            m_jointStateMsg.effort[i] = currentJointEffort;
-            i++;
-        }
-        m_jointStatePublisher->publish(m_jointStateMsg);
+        InstallPhysicalCallback();
     }
 
-    void JointStatePublisher::OnTick(float deltaTime)
+    void JointStatePublisher::OnPhysicsSimulationFinished([[maybe_unused]] AzPhysics::SceneHandle sceneHandle, float deltaTime)
     {
         AZ_Assert(m_configuration.m_frequency > 0.f, "JointPublisher frequency must be greater than zero");
         auto frameTime = 1.f / m_configuration.m_frequency;

Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h

@@ -10,6 +10,8 @@
 
 #include <AzCore/Component/EntityId.h>
 #include <ROS2/Communication/PublisherConfiguration.h>
+#include <ROS2/Manipulation/JointInfo.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 
@@ -24,13 +26,13 @@ namespace ROS2
 
     //! A class responsible for publishing the joint positions on ROS2 /joint_states topic.
     //!< @see <a href="https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html">jointState message</a>.
-    class JointStatePublisher
+    class JointStatePublisher : public ROS2::Utils::PhysicsCallbackHandler
     {
     public:
         JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context);
+        virtual ~JointStatePublisher() = default;
 
-        //! Update time tick. This will result in state publishing if timing matches frequency.
-        void OnTick(float deltaTime);
+        void InitializePublisher(AZ::EntityId entityId);
 
     private:
         void PublishMessage();
@@ -41,5 +43,11 @@ namespace ROS2
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> m_jointStatePublisher;
         sensor_msgs::msg::JointState m_jointStateMsg;
         float m_timeElapsedSinceLastTick = 0.0f;
+
+        AZStd::vector<AZStd::string> m_jointNames;
+        AZStd::vector<JointInfo> m_jointInfos;
+
+        // ROS2::Utils::PhysicsCallbackHandler overrides ...
+        void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp

@@ -10,6 +10,7 @@
 #include "Controllers/JointsArticulationControllerComponent.h"
 #include "Controllers/JointsPIDControllerComponent.h"
 #include "JointStatePublisher.h"
+#include "ManipulationUtils.h"
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Component/TransformBus.h>
 #include <AzCore/Debug/Trace.h>
@@ -211,15 +212,9 @@ namespace ROS2
         return m_manipulationJoints;
     }
 
-    AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const AZStd::string& jointName)
+    AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const JointInfo& jointInfo)
     {
-        if (!m_manipulationJoints.contains(jointName))
-        {
-            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
-        }
-
-        auto jointInfo = m_manipulationJoints.at(jointName);
-        float position{ 0 };
+        float position{ 0.f };
         if (jointInfo.m_isArticulation)
         {
             PhysX::ArticulationJointRequestBus::EventResult(
@@ -235,7 +230,7 @@ namespace ROS2
         return AZ::Success(position);
     }
 
-    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const AZStd::string& jointName)
+    AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const AZStd::string& jointName)
     {
         if (!m_manipulationJoints.contains(jointName))
         {
@@ -243,7 +238,13 @@ namespace ROS2
         }
 
         auto jointInfo = m_manipulationJoints.at(jointName);
-        float velocity{ 0 };
+
+        return GetJointPosition(jointInfo);
+    }
+
+    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const JointInfo& jointInfo)
+    {
+        float velocity{ 0.f };
         if (jointInfo.m_isArticulation)
         {
             PhysX::ArticulationJointRequestBus::EventResult(
@@ -259,12 +260,23 @@ namespace ROS2
         return AZ::Success(velocity);
     }
 
+    AZ::Outcome<JointVelocity, AZStd::string> JointsManipulationComponent::GetJointVelocity(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        return GetJointVelocity(jointInfo);
+    }
+
     JointsManipulationRequests::JointsPositionsMap JointsManipulationComponent::GetAllJointsPositions()
     {
         JointsManipulationRequests::JointsPositionsMap positions;
         for (const auto& [jointName, jointInfo] : m_manipulationJoints)
         {
-            positions[jointName] = GetJointPosition(jointName).GetValue();
+            positions[jointName] = GetJointPosition(jointInfo).GetValue();
         }
         return positions;
     }
@@ -274,11 +286,18 @@ namespace ROS2
         JointsManipulationRequests::JointsVelocitiesMap velocities;
         for (const auto& [jointName, jointInfo] : m_manipulationJoints)
         {
-            velocities[jointName] = GetJointVelocity(jointName).GetValue();
+            velocities[jointName] = GetJointVelocity(jointInfo).GetValue();
         }
         return velocities;
     }
 
+    AZ::Outcome<JointEffort, AZStd::string> JointsManipulationComponent::GetJointEffort(const JointInfo& jointInfo)
+    {
+        auto jointStateData = Utils::GetJointState(jointInfo);
+
+        return AZ::Success(jointStateData.effort);
+    }
+
     AZ::Outcome<JointEffort, AZStd::string> JointsManipulationComponent::GetJointEffort(const AZStd::string& jointName)
     {
         if (!m_manipulationJoints.contains(jointName))
@@ -287,47 +306,7 @@ namespace ROS2
         }
 
         auto jointInfo = m_manipulationJoints.at(jointName);
-        float effort{ 0 };
-
-        if (!jointInfo.m_isArticulation)
-        {
-            // The joint isn't controlled by JointsArticulationControllerComponent.
-            // There is no easy way to calculate the effort.
-            return AZ::Success(effort);
-        }
-
-        bool is_acceleration_driven{ false };
-        PhysX::ArticulationJointRequestBus::EventResult(
-            is_acceleration_driven,
-            jointInfo.m_entityComponentIdPair.GetEntityId(),
-            &PhysX::ArticulationJointRequests::IsAccelerationDrive,
-            jointInfo.m_axis);
-
-        if (is_acceleration_driven)
-        {
-            // The formula below in this case will calculate acceleration, not effort.
-            // In this case we can't calculate the effort without PhysX engine support.
-            return AZ::Success(effort);
-        }
-
-        float stiffness{ 0 }, damping{ 0 }, target_pos{ 0 }, position{ 0 }, target_vel{ 0 }, velocity{ 0 }, max_force{ 0 };
-        PhysX::ArticulationJointRequestBus::Event(
-            jointInfo.m_entityComponentIdPair.GetEntityId(),
-            [&](PhysX::ArticulationJointRequests* articulationJointRequests)
-            {
-                stiffness = articulationJointRequests->GetDriveStiffness(jointInfo.m_axis);
-                damping = articulationJointRequests->GetDriveDamping(jointInfo.m_axis);
-                target_pos = articulationJointRequests->GetDriveTarget(jointInfo.m_axis);
-                position = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
-                target_vel = articulationJointRequests->GetDriveTargetVelocity(jointInfo.m_axis);
-                velocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
-                max_force = articulationJointRequests->GetMaxForce(jointInfo.m_axis);
-            });
-        effort = stiffness * -(position - target_pos) + damping * (target_vel - velocity);
-
-        effort = AZ::GetClamp(effort, -max_force, max_force);
-
-        return AZ::Success(effort);
+        return GetJointEffort(jointInfo);
     }
 
     JointsManipulationRequests::JointsEffortsMap JointsManipulationComponent::GetAllJointsEfforts()
@@ -335,7 +314,7 @@ namespace ROS2
         JointsManipulationRequests::JointsEffortsMap efforts;
         for (const auto& [jointName, jointInfo] : m_manipulationJoints)
         {
-            efforts[jointName] = GetJointEffort(jointName).GetValue();
+            efforts[jointName] = GetJointEffort(jointInfo).GetValue();
         }
         return efforts;
     }
@@ -462,8 +441,8 @@ namespace ROS2
                 AZ::TickBus::Handler::BusDisconnect();
                 return;
             }
+            m_jointStatePublisher->InitializePublisher(GetEntityId());
         }
-        m_jointStatePublisher->OnTick(deltaTime);
         MoveToSetPositions(deltaTime);
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h

@@ -71,6 +71,10 @@ namespace ROS2
 
         void MoveToSetPositions(float deltaTime);
 
+        AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const JointInfo& jointInfo);
+        AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const JointInfo& jointInfo);
+        AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const JointInfo& jointInfo);
+
         AZStd::unique_ptr<JointStatePublisher> m_jointStatePublisher;
         PublisherConfiguration m_jointStatePublisherConfiguration;
         ManipulationJoints m_manipulationJoints;

Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.cpp

@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "ManipulationUtils.h"
+#include <PhysX/ArticulationJointBus.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+
+namespace ROS2::Utils
+{
+    JointStateData GetJointState(const JointInfo& jointInfo)
+    {
+        JointStateData result;
+
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                [&](PhysX::ArticulationJointRequests* articulationJointRequests)
+                {
+                    result.position = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
+                    result.velocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
+                    const bool is_acceleration_driven = articulationJointRequests->IsAccelerationDrive(jointInfo.m_axis);
+                    if (!is_acceleration_driven)
+                    {
+                        const float stiffness = articulationJointRequests->GetDriveStiffness(jointInfo.m_axis);
+                        const float damping = articulationJointRequests->GetDriveDamping(jointInfo.m_axis);
+                        const float targetPosition = articulationJointRequests->GetDriveTarget(jointInfo.m_axis);
+                        const float targetVelocity = articulationJointRequests->GetDriveTargetVelocity(jointInfo.m_axis);
+                        const float maxEffort = articulationJointRequests->GetMaxForce(jointInfo.m_axis);
+                        result.effort = stiffness * -(result.position - targetPosition) + damping * (targetVelocity - result.velocity);
+                        result.effort = AZ::GetClamp(result.effort, -maxEffort, maxEffort);
+                    }
+                });
+        }
+        else
+        {
+            PhysX::JointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair,
+                [&](PhysX::JointRequests* jointRequests)
+                {
+                    result.position = jointRequests->GetPosition();
+                    result.velocity = jointRequests->GetVelocity();
+                });
+        }
+        return result;
+    }
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Manipulation/ManipulationUtils.h

@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+#include <ROS2/Manipulation/JointInfo.h>
+
+namespace ROS2::Utils
+{
+    struct JointStateData
+    {
+        float position{ 0.f };
+        float velocity{ 0.f };
+        float effort{ 0.f };
+    };
+
+    //! Get the current joint state
+    //! @param jointInfo Info of the joint we want to get data of.
+    //! @return Data with the current joint state.
+    JointStateData GetJointState(const JointInfo& jointInfo);
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp

@@ -51,18 +51,25 @@ namespace ROS2
 
     void ROS2OdometrySensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
+        required.push_back(AZ_CRC_CE("PhysicsDynamicRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
     }
 
     // ROS2SensorComponent overrides ...
     void ROS2OdometrySensorComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
     }
 
     void ROS2OdometrySensorComponent::OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle)
     {
+        AzPhysics::RigidBody* rigidBody = nullptr;
+        AZ::EntityId entityId = GetEntityId();
+        Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(rigidBody, "Entity %s does not have rigid body.", entityId.ToString().c_str());
+
+        m_bodyHandle = rigidBody->m_bodyHandle;
+
         auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
         auto* simulatedBodyPtr = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, m_bodyHandle);
         auto rigidbodyPtr = azrtti_cast<AzPhysics::RigidBody*>(simulatedBodyPtr);

Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.h

@@ -13,9 +13,10 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzFramework/Physics/Common/PhysicsEvents.h>
 #include <AzFramework/Physics/PhysicsScene.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/publisher.hpp>
 
@@ -53,5 +54,8 @@ namespace ROS2
         // ROS2::Utils::PhysicsCallbackHandler overrides ...
         void OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime) override;
         void OnPhysicsInitialization(AzPhysics::SceneHandle sceneHandle) override;
+
+        //! Handler to simulated physical body
+        AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp

@@ -67,14 +67,13 @@ namespace ROS2
 
     void ROS2WheelOdometryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
     {
-        required.push_back(AZ_CRC_CE("PhysicsRigidBodyService"));
         required.push_back(AZ_CRC_CE("ROS2Frame"));
         required.push_back(AZ_CRC_CE("SkidSteeringModelService"));
     }
 
     void ROS2WheelOdometryComponent::SetupRefreshLoop()
     {
-        InstallPhysicalCallback(m_entity->GetId());
+        InstallPhysicalCallback();
     }
 
     void ROS2WheelOdometryComponent::OnPhysicsSimulationFinished(AzPhysics::SceneHandle sceneHandle, float deltaTime)

Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h

@@ -14,7 +14,7 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
-#include <Utilities/PhysicsCallbackHandler.h>
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/publisher.hpp>
 

Gems/ROS2/Code/Source/Utilities/ArticulationsUtilities.cpp

@@ -6,6 +6,7 @@
  *
  */
 
+#include <AzFramework/Physics/PhysicsScene.h>
 #include "ArticulationsUtilities.h"
 #include "Source/ArticulationLinkComponent.h"
 
@@ -52,4 +53,24 @@ namespace ROS2::Utils
         }
         return GetRootOfArticulation(parentEntity->GetId());
     }
+
+    AZStd::unordered_map<AZ::EntityId, AzPhysics::SimulatedBodyHandle> GetSimulatedBodyHandles(AzPhysics::SceneHandle sceneHandle, AZ::EntityId entityId)
+    {
+        AZ::EntityId rootArticulationEntity = GetRootOfArticulation(entityId);
+        AZ::Entity* rootEntity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(rootEntity, &AZ::ComponentApplicationRequests::FindEntity, rootArticulationEntity);
+
+        const PhysX::ArticulationLinkComponent* component = rootEntity->FindComponent<PhysX::ArticulationLinkComponent>();
+        const AZStd::vector<AzPhysics::SimulatedBodyHandle> articulationHandles = component->GetSimulatedBodyHandles();
+
+        AZStd::unordered_map<AZ::EntityId, AzPhysics::SimulatedBodyHandle> result;
+        for (const auto& articulationHandle : articulationHandles)
+        {
+            auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+            const auto* body = sceneInterface->GetSimulatedBodyFromHandle(sceneHandle, articulationHandle);
+            result.insert({body->GetEntityId(), articulationHandle});
+        }
+
+        return result;
+    }
 } // namespace ROS2::Utils

Gems/ROS2/Code/Source/Utilities/ArticulationsUtilities.h

@@ -8,12 +8,18 @@
 
 #pragma once
 #include <AzCore/Component/Entity.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>
 
 namespace ROS2::Utils
 {
     //! Retrieve root of articulation for given entity. If the entity is not part of an articulation, the invalid entity id is returned.
     //! @param entityId The entity to get the root of.
-    //! @return The root of articulation
+    //! @return The root of articulation.
     AZ::EntityId GetRootOfArticulation(AZ::EntityId entityId);
 
+    //! Get handles to all the articulation links in an articulation tree.
+    //! @param sceneHandle A handle to the scene.
+    //! @param entityId Any entity in the articulation tree.
+    //! @return Handles to all the articulation links in the tree.
+    AZStd::unordered_map<AZ::EntityId, AzPhysics::SimulatedBodyHandle> GetSimulatedBodyHandles(AzPhysics::SceneHandle sceneHandle, AZ::EntityId entityId);
 } // namespace ROS2::Utils

Gems/ROS2/Code/Source/Utilities/PhysicsCallbackHandler.cpp

@@ -5,36 +5,22 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#include "PhysicsCallbackHandler.h"
-#include "AzFramework/Physics/RigidBodyBus.h"
-#include "AzFramework/Physics/SimulatedBodies/RigidBody.h"
+#include <ROS2/Utilities/PhysicsCallbackHandler.h>
 
 namespace ROS2::Utils
 {
-    void PhysicsCallbackHandler::InstallPhysicalCallback(AZ::EntityId entityId)
+    void PhysicsCallbackHandler::InstallPhysicalCallback()
     {
-        m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
         m_onSceneSimulationStart = AzPhysics::SceneEvents::OnSceneSimulationStartHandler(
-            [this, entityId](AzPhysics::SceneHandle sceneHandle, float deltaTime)
+            [this](AzPhysics::SceneHandle sceneHandle, float deltaTime)
             {
-                AzPhysics::RigidBody* rigidBody = nullptr;
-                Physics::RigidBodyRequestBus::EventResult(rigidBody, entityId, &Physics::RigidBodyRequests::GetRigidBody);
-                AZ_Assert(rigidBody, "Entity %s does not have rigid body.", entityId.ToString().c_str());
-                if (rigidBody)
-                {
-                    m_bodyHandle = rigidBody->m_bodyHandle;
-                    OnPhysicsInitialization(sceneHandle);
-                    m_onSceneSimulationStart.Disconnect();
-                }
+                OnPhysicsInitialization(sceneHandle);
+                m_onSceneSimulationStart.Disconnect();
             });
 
         m_onSceneSimulationEvent = AzPhysics::SceneEvents::OnSceneSimulationFinishHandler(
             [this](AzPhysics::SceneHandle sceneHandle, float deltaTime)
             {
-                if (m_bodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
-                {
-                    return;
-                }
                 OnPhysicsSimulationFinished(sceneHandle, deltaTime);
             });
 

Gems/ROS2/Code/ros2_files.cmake

@@ -85,6 +85,8 @@ set(FILES
         Source/Manipulation/JointsTrajectoryComponent.h
         Source/Manipulation/FollowJointTrajectoryActionServer.cpp
         Source/Manipulation/FollowJointTrajectoryActionServer.h
+        Source/Manipulation/ManipulationUtils.h
+        Source/Manipulation/ManipulationUtils.cpp
         Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp
         Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp
         Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp
@@ -134,7 +136,6 @@ set(FILES
         Source/Utilities/JointUtilities.h
         Source/Utilities/Controllers/PidConfiguration.cpp
         Source/Utilities/PhysicsCallbackHandler.cpp
-        Source/Utilities/PhysicsCallbackHandler.h
         Source/Utilities/ROS2Conversions.cpp
         Source/Utilities/ROS2Names.cpp
         Source/VehicleDynamics/AxleConfiguration.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -38,6 +38,7 @@ set(FILES
         Include/ROS2/Sensor/SensorConfiguration.h
         Include/ROS2/Spawner/SpawnerBus.h
         Include/ROS2/Utilities/Controllers/PidConfiguration.h
+        Include/ROS2/Utilities/PhysicsCallbackHandler.h
         Include/ROS2/Utilities/ROS2Conversions.h
         Include/ROS2/Utilities/ROS2Names.h
         Include/ROS2/VehicleDynamics/VehicleInputControlBus.h