Further improvements

Signed-off-by: Michał Pełka <[email protected]>
o3de · Jul 12, 2024 · afdef2c · afdef2c
1 parent a712a7a
commit afdef2c
Show file tree

Hide file tree

Showing 8 changed files with 18 additions and 14 deletions.
diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h
@@ -60,6 +60,6 @@ namespace ROS2
         // AZ::TickBus overrides
         void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
 
-        builtin_interfaces::msg::Time m_lastTickTime; //!< ROS 2 Timestamp during last OnTick call
+        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
@@ -14,6 +14,7 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <builtin_interfaces/msg/time.hpp>
+
 namespace ROS2
 {
     //! Utility class for conversions between ROS2 types and O3DE (AZ::) types.

diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp
@@ -183,7 +183,7 @@ namespace ROS2
         publisherContext.m_entityId = GetEntityId();
 
         m_jointStatePublisher = AZStd::make_unique<JointStatePublisher>(m_jointStatePublisherConfiguration, publisherContext);
-        m_lastTickTime = ROS2Interface::Get()->GetROSTimestamp();
+        m_lastTickTimestamp = ROS2Interface::Get()->GetROSTimestamp();
         AZ::TickBus::Handler::BusConnect();
         JointsManipulationRequestBus::Handler::BusConnect(GetEntityId());
     }
@@ -450,8 +450,8 @@ namespace ROS2
             m_jointStatePublisher->InitializePublisher();
         }
         auto simTimestamp = ROS2Interface::Get()->GetROSTimestamp();
-        float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTime, simTimestamp);
+        float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTimestamp, simTimestamp);
         MoveToSetPositions(deltaSimTime);
-        m_lastTickTime = simTimestamp;
+        m_lastTickTimestamp = simTimestamp;
     }
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h
@@ -86,6 +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_lastTickTime; //!< ROS 2 Timestamp during last OnTick call
+        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
@@ -25,7 +25,7 @@ namespace ROS2
         m_followTrajectoryServer = AZStd::make_unique<FollowJointTrajectoryActionServer>(namespacedAction, GetEntityId());
         AZ::TickBus::Handler::BusConnect();
         JointsTrajectoryRequestBus::Handler::BusConnect(GetEntityId());
-        m_lastTickTime = ROS2Interface::Get()->GetROSTimestamp();
+        m_lastTickTimestamp = ROS2Interface::Get()->GetROSTimestamp();
     }
 
     ManipulationJoints& JointsTrajectoryComponent::GetManipulationJoints()
@@ -256,8 +256,8 @@ namespace ROS2
             return;
         }
         const auto simTimestamp = ROS2Interface::Get()->GetROSTimestamp();
-        const float deltaSimulatedTime = ROS2Conversions::GetTimeDifference(simTimestamp, m_lastTickTime);
-        m_lastTickTime = simTimestamp;
+        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,6 +66,6 @@ namespace ROS2
         rclcpp::Time m_trajectoryExecutionStartTime;
         ManipulationJoints m_manipulationJoints;
         bool m_trajectoryInProgress{ false };
-        builtin_interfaces::msg::Time m_lastTickTime;
+        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,14 +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_lastTickTime = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+        m_lastTickTimestamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
         AZ::TickBus::Handler::BusConnect();
         ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
         AZ::EntityBus::Handler::BusConnect(GetEntityId());
@@ -88,10 +89,10 @@ namespace ROS2
         PhysX::JointRequestBus::EventResult(m_currentPosition, m_jointComponentIdPair, &PhysX::JointRequests::GetPosition);
 
         const auto timestamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
-        const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTime, timestamp);
+        const float deltaSimTime = ROS2Conversions::GetTimeDifference(m_lastTickTimestamp, timestamp);
         const float setSpeed = CalculateMotorSpeed(deltaSimTime);
         PhysX::JointRequestBus::Event(m_jointComponentIdPair, &PhysX::JointRequests::SetVelocity, setSpeed);
-        m_lastTickTime = timestamp;
+        m_lastTickTimestamp = timestamp;
     }
 
     void JointMotorControllerComponent::OnEntityActivated(const AZ::EntityId& entityId)

diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp
@@ -192,8 +192,10 @@ namespace ROS2
         }
     }
 
-    void ROS2SystemComponent::OnTick([[maybe_unused]]float deltaTime, [[maybe_unused]]AZ::ScriptTimePoint time) {
-        if (rclcpp::ok()) {
+    void ROS2SystemComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (rclcpp::ok())
+        {
             m_dynamicTFBroadcaster->sendTransform(m_frameTransforms);
             m_frameTransforms.clear();