Use simulation time to compute time difference. (o3de#714)

Signed-off-by: Michał Pełka <[email protected]> Co-authored-by: Jan Hanca <[email protected]>
RobotecAI · Aug 13, 2024 · e57f3e3 · e57f3e3
1 parent dcda6e4
commit e57f3e3
Show file tree

Hide file tree

Showing 8 changed files with 34 additions and 6 deletions.
diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h
@@ -13,6 +13,7 @@
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ImGuiBus.h>
 #include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h>
+#include <builtin_interfaces/msg/time.hpp>
 
 namespace ROS2
 {
@@ -58,5 +59,7 @@ namespace ROS2
 
         // AZ::TickBus overrides
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        builtin_interfaces::msg::Time m_lastTickTimestamp; //!< ROS 2 Timestamp during last OnTick call
     };
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h b/Gems/ROS2/Code/Include/ROS2/Utilities/ROS2Conversions.h
@@ -13,6 +13,7 @@
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <builtin_interfaces/msg/time.hpp>
 
 namespace ROS2
 {
@@ -28,5 +29,6 @@ namespace ROS2
         AZ::Vector3 FromROS2Point(const geometry_msgs::msg::Point& ros2point);
         AZ::Quaternion FromROS2Quaternion(const geometry_msgs::msg::Quaternion& ros2quaternion);
         std::array<double, 9> ToROS2Covariance(const AZ::Matrix3x3& covariance);
+        float GetTimeDifference(const builtin_interfaces::msg::Time& start, const builtin_interfaces::msg::Time& end);
     }; // namespace ROS2Conversions
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp
@@ -22,6 +22,7 @@
 #include <Source/HingeJointComponent.h>
 #include <Source/PrismaticJointComponent.h>
 #include <Utilities/ArticulationsUtilities.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
 
 namespace ROS2
 {
@@ -182,7 +183,7 @@ namespace ROS2
         publisherContext.m_entityId = GetEntityId();
 
         m_jointStatePublisher = AZStd::make_unique<JointStatePublisher>(m_jointStatePublisherConfiguration, publisherContext);
-
+        m_lastTickTimestamp = ROS2Interface::Get()->GetROSTimestamp();
         AZ::TickBus::Handler::BusConnect();
         JointsManipulationRequestBus::Handler::BusConnect(GetEntityId());
     }
@@ -422,7 +423,7 @@ namespace ROS2
         return frameComponent->GetNamespace();
     }
 
-    void JointsManipulationComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    void JointsManipulationComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
     {
         if (m_manipulationJoints.empty())
         {
@@ -448,6 +449,9 @@ namespace ROS2
             }
             m_jointStatePublisher->InitializePublisher();
         }
-        MoveToSetPositions(deltaTime);
+        auto simTimestamp = ROS2Interface::Get()->GetROSTimestamp();
+        float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTimestamp, simTimestamp);
+        MoveToSetPositions(deltaSimTime);
+        m_lastTickTimestamp = simTimestamp;
     }
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h
@@ -86,5 +86,6 @@ namespace ROS2
         ManipulationJoints m_manipulationJoints; //!< Map of JointInfo where the key is a joint name (with namespace included)
         AZStd::vector<AZStd::pair<AZStd::string, float>> 
             m_initialPositions; //!< Initial positions per joint name (without namespace included)
+        builtin_interfaces::msg::Time m_lastTickTimestamp; //!< ROS 2 Timestamp during last OnTick call
     };
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp
@@ -13,6 +13,7 @@
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Utilities/ROS2Names.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
 
 namespace ROS2
 {
@@ -24,6 +25,7 @@ namespace ROS2
         m_followTrajectoryServer = AZStd::make_unique<FollowJointTrajectoryActionServer>(namespacedAction, GetEntityId());
         AZ::TickBus::Handler::BusConnect();
         JointsTrajectoryRequestBus::Handler::BusConnect(GetEntityId());
+        m_lastTickTimestamp = ROS2Interface::Get()->GetROSTimestamp();
     }
 
     ManipulationJoints& JointsTrajectoryComponent::GetManipulationJoints()
@@ -246,14 +248,17 @@ namespace ROS2
         }
     }
 
-    void JointsTrajectoryComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    void JointsTrajectoryComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
     {
         if (m_manipulationJoints.empty())
         {
             GetManipulationJoints();
             return;
         }
-        uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
+        const auto simTimestamp = ROS2Interface::Get()->GetROSTimestamp();
+        const float deltaSimulatedTime = ROS2Conversions::GetTimeDifference(simTimestamp, m_lastTickTimestamp);
+        m_lastTickTimestamp = simTimestamp;
+        const uint64_t deltaTimeNs = deltaSimulatedTime * 1'000'000'000;
         FollowTrajectory(deltaTimeNs);
         UpdateFeedback();
     }

diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h
@@ -66,5 +66,6 @@ namespace ROS2
         rclcpp::Time m_trajectoryExecutionStartTime;
         ManipulationJoints m_manipulationJoints;
         bool m_trajectoryInProgress{ false };
+        builtin_interfaces::msg::Time m_lastTickTimestamp; //!< ROS 2 Timestamp during last OnTick call
     };
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp
@@ -12,12 +12,15 @@
 #include <PhysX/Joint/PhysXJointRequestsBus.h>
 #include <PrismaticJointComponent.h>
 #include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Conversions.h>
 #include <imgui/imgui.h>
 
 namespace ROS2
 {
     void JointMotorControllerComponent::Activate()
     {
+        m_lastTickTimestamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
         AZ::TickBus::Handler::BusConnect();
         ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
         AZ::EntityBus::Handler::BusConnect(GetEntityId());
@@ -84,8 +87,12 @@ namespace ROS2
         }
 
         PhysX::JointRequestBus::EventResult(m_currentPosition, m_jointComponentIdPair, &PhysX::JointRequests::GetPosition);
-        float setSpeed = CalculateMotorSpeed(deltaTime);
+
+        const auto timestamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+        const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTimestamp, timestamp);
+        const float setSpeed = CalculateMotorSpeed(deltaSimTime);
         PhysX::JointRequestBus::Event(m_jointComponentIdPair, &PhysX::JointRequests::SetVelocity, setSpeed);
+        m_lastTickTimestamp = timestamp;
     }
 
     void JointMotorControllerComponent::OnEntityActivated(const AZ::EntityId& entityId)

diff --git a/Gems/ROS2/Code/Source/Utilities/ROS2Conversions.cpp b/Gems/ROS2/Code/Source/Utilities/ROS2Conversions.cpp
@@ -83,4 +83,9 @@ namespace ROS2
         return ros2Covariance;
     }
 
+    float ROS2Conversions::GetTimeDifference(const builtin_interfaces::msg::Time &start,
+                                             const builtin_interfaces::msg::Time &end) {
+        return static_cast<float>(end.sec - start.sec) + static_cast<float>(end.nanosec - start.nanosec) / 1e9;
+    }
+
 } // namespace ROS2