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,15 +230,21 @@ 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))
         {
             return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
         }
 
         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,55 +306,15 @@ 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()
     {
         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;