diff --git a/Gems/SensorDebug/Code/CMakeLists.txt b/Gems/SensorDebug/Code/CMakeLists.txt index d0eb8007..abb8a8ff 100644 --- a/Gems/SensorDebug/Code/CMakeLists.txt +++ b/Gems/SensorDebug/Code/CMakeLists.txt @@ -54,6 +54,7 @@ ly_add_target( Gem::ImGui Gem::ROS2.Static Gem::ROS2.API + Gem::PhysX5.Static ) # Here add ${gem_name} target, it depends on the Private Object library and Public API interface diff --git a/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.cpp b/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.cpp index 345fd735..e13b9eef 100644 --- a/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.cpp +++ b/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.cpp @@ -4,6 +4,10 @@ #include #include +#include + +#include +#include namespace SensorDebug { @@ -20,10 +24,13 @@ namespace SensorDebug void SensorDebugSystemComponent::Activate() { ImGui::ImGuiUpdateListenerBus::Handler::BusConnect(); + AZ::TickBus::Handler::BusConnect(); + GetPhysXConfig(); } void SensorDebugSystemComponent::Deactivate() { + AZ::TickBus::Handler::BusDisconnect(); ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect(); } @@ -96,6 +103,29 @@ namespace SensorDebug AZ_Printf("TestComponent", "Found %d sensor entities", m_sensorEntities.size()); } + AzPhysics::Scene* SensorDebugSystemComponent::GetScene() + { + if (m_scene) + { + return m_scene; + } + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "No physics system."); + + AzPhysics::SceneInterface* sceneInterface = AZ::Interface::Get(); + AZ_Assert(sceneInterface, "No scene intreface."); + + AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName); + if (defaultSceneHandle == AzPhysics::InvalidSceneHandle) + { + return nullptr; + } + AzPhysics::Scene* scene = sceneInterface->GetScene(defaultSceneHandle); + AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene pointer."); + m_scene = scene; + return scene; + } + void SensorDebugSystemComponent::FindSensorsWithGivenType(const char* typeId) { ClearSensors(); @@ -126,9 +156,118 @@ namespace SensorDebug AZ_Printf("TestComponent", "Found %d sensor entities", m_sensorEntities.size()); } + void SensorDebugSystemComponent::Pause(bool isPaused) + { + GetScene()->SetEnabled(!isPaused); + } + + void SensorDebugSystemComponent::UpdatePhysXConfig() + { + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "No physics system."); + auto* physicsSystemConfigurationPtr = physicsSystem->GetConfiguration(); + auto* physicsSystemConfiguration = azdynamic_cast(physicsSystemConfigurationPtr); + AZ_Assert(physicsSystemConfiguration, "Invalid physics system configuration pointer, a new Physics system in O3DE????"); + physicsSystem->UpdateConfiguration(&m_modifiedPhysXConfig, true); + } + + void SensorDebugSystemComponent::GetPhysXConfig() + { + AzPhysics::SystemInterface* physicsSystem = AZ::Interface::Get(); + AZ_Assert(physicsSystem, "No physics system."); + auto* physicsSystemConfigurationPtr = physicsSystem->GetConfiguration(); + auto* physicsSystemConfiguration = azdynamic_cast(physicsSystemConfigurationPtr); + AZ_Assert(physicsSystemConfiguration, "Invalid physics system configuration pointer, a new Physics system in O3DE????"); + m_modifiedPhysXConfig = *physicsSystemConfiguration; + } + + AZStd::string GetSecondAnimation(double value) + { + double part = value - AZStd::floor(value); + size_t numStars = static_cast(part * 60); + + AZStd::string anim (61, ' '); + numStars = AZStd::min(numStars, anim.length()); + for (size_t i = 0; i GetROSTimestamp() : builtin_interfaces::msg::Time(); + const float ros2tsSec = ros2ts.sec + ros2ts.nanosec / 1e9; + auto ros2Node = ros2Intreface ? ROS2::ROS2Interface::Get()->GetNode() : nullptr; + auto nodeTime = ros2Node ? ros2Node->now() : rclcpp::Time(0, 0); + const double ros2nodetsSec = nodeTime.seconds() + nodeTime.nanoseconds() / 1e9; + + auto timeSystem = AZ::Interface::Get(); + const auto elapsedTime = timeSystem ? static_cast(timeSystem->GetElapsedTimeUs()) / 1e6 : 0.0; + + ImGui::Text("Current ROS 2 time (Gem) : %f %s", ros2tsSec, GetSecondAnimation(ros2tsSec).c_str()); + ImGui::Text("Current ROS 2 time (Node) : %f %s", ros2nodetsSec, GetSecondAnimation(ros2nodetsSec).c_str()); + ImGui::Text("Current O3DE time : %f %s", elapsedTime, GetSecondAnimation(elapsedTime).c_str()); + + ImGui::Separator(); + ImGui::Text("PhysX"); + ImGui::InputFloat("Fixed timestamp", &m_modifiedPhysXConfig.m_fixedTimestep, 0.0f, 0.0f, "%.6f"); + ImGui::InputFloat("Max timestamp", &m_modifiedPhysXConfig.m_maxTimestep, 0.0f, 0.0f, "%.6f"); + if (ImGui::Button("Update PhysX Config")) + { + UpdatePhysXConfig(); + } + ImGui::SameLine(); + if (ImGui::Button("Pause")) + { + Pause(true); + } + ImGui::SameLine(); + if (ImGui::Button("Unpause")) + { + Pause(false); + } + ImGui::Separator(); + ImGui::Text("Atom"); + + ImGui::InputFloat("Application Max FPS", &m_maxFPS); + ImGui::SameLine(); + if (ImGui::Button("Set sys_MaxFPS")) + { + // disable vsync + AZStd::string commandVsync = AZStd::string::format("vsync_interval=0"); + AzFramework::ConsoleRequestBus::Broadcast(&AzFramework::ConsoleRequests::ExecuteConsoleCommand, commandVsync.c_str()); + AZStd::string commandMaxFps = AZStd::string::format("sys_MaxFPS=%f", m_maxFPS); + AzFramework::ConsoleRequestBus::Broadcast(&AzFramework::ConsoleRequests::ExecuteConsoleCommand, commandMaxFps.c_str()); + m_appFrequencies.clear(); + } + + float freqencySum = 0.0f; + for (const auto& freq : m_appFrequencies) + { + freqencySum += freq; + } + const float averageFrequency = freqencySum / static_cast(m_appFrequencies.size()); + // compute std deviation + float variance = 0.0f; + for (const auto& freq : m_appFrequencies) + { + variance += (freq - averageFrequency) * (freq - averageFrequency); + } + float stdDeviation = sqrt(variance / static_cast(m_appFrequencies.size())); + ImGui::Separator(); + ImGui::PlotHistogram("App Actual Frequency", m_appFrequencies.data(), static_cast(m_appFrequencies.size()), 0); + ImGui::Text("App Actual Frequency: %.2f Hz [ std_dev = %.2f ]", averageFrequency, stdDeviation); + ImGui::SameLine(); + if (ImGui::Button("reset stats")) + { + m_appFrequencies.clear(); + } + ImGui::Separator(); if (ImGui::Button("Refresh with EnumerateHandlers(o3de bus API)")) { FindSensorsWithBusAPI(); @@ -178,7 +317,7 @@ namespace SensorDebug { FindSensorsWithGivenType(ROS2::ROS2OdometrySensorComponent); } - + ImGui::InputInt("History Size", &m_historySize); for (auto& sensorEntity : m_sensorEntities) { ImGui::Separator(); @@ -189,7 +328,42 @@ namespace SensorDebug float frequency = 0.0f; ROS2::SensorConfigurationRequestBus::EventResult( frequency, sensorEntity, &ROS2::SensorConfigurationRequest::GetEffectiveFrequency); - ImGui::Text("%s : %s effective Freq: %f Hz", entityName.c_str(), sensorName.c_str(), frequency); + + m_sensorFrequencyHistory[sensorEntity].push_back(frequency); + auto& sensorFrequencyHistory = m_sensorFrequencyHistory[sensorEntity]; + if (sensorFrequencyHistory.size() > m_historySize) + { + sensorFrequencyHistory.erase(sensorFrequencyHistory.begin()); + } + float freqencySum = 0.0f; + for (const auto& freq : sensorFrequencyHistory) + { + freqencySum += freq; + } + const float averageFrequency = freqencySum / static_cast(sensorFrequencyHistory.size()); + // compute std deviation + float variance = 0.0f; + for (const auto& freq : sensorFrequencyHistory) + { + variance += (freq - averageFrequency) * (freq - averageFrequency); + } + float stdDeviation = sqrt(variance / static_cast(sensorFrequencyHistory.size())); + const AZStd::string histogramNameWithCookie = AZStd::string::format("Histogram%s", cookie.c_str()); + + ImGui::PlotHistogram( + histogramNameWithCookie.c_str(), sensorFrequencyHistory.data(), static_cast(sensorFrequencyHistory.size()), 0); + ImGui::SameLine(); + const AZStd::string resetButton = AZStd::string::format("reset stats%s", cookie.c_str()); + if (ImGui::Button(resetButton.c_str())) + { + sensorFrequencyHistory.clear(); + } + ImGui::Text( + "%s : %s effective Freq: %.2f Hz [ std_dev = %.2f ]", + entityName.c_str(), + sensorName.c_str(), + averageFrequency, + stdDeviation); AZStd::string buttonNameEna = AZStd::string::format("Enable%s", cookie.c_str()); AZStd::string buttonNameDis = AZStd::string::format("Disable%s", cookie.c_str()); @@ -230,7 +404,7 @@ namespace SensorDebug ROS2::SensorConfigurationRequestBus::Event(sensorEntity, &ROS2::SensorConfigurationRequest::SetVisualizeEnabled, false); } AZStd::string freqName = AZStd::string::format("Frequency%s", cookie.c_str()); - ImGui::DragFloat(freqName.c_str(), &m_sensorFrequencies[sensorEntity], 0.1f, 0.1f, 500.0f); + ImGui::InputFloat(freqName.c_str(), &m_sensorFrequencies[sensorEntity]); ImGui::SameLine(); AZStd::string buttonSetFreq = AZStd::string::format("Set Frequency%s", cookie.c_str()); if (ImGui::Button(buttonSetFreq.c_str())) @@ -242,4 +416,13 @@ namespace SensorDebug ImGui::End(); } + + void SensorDebugSystemComponent::OnTick(float deltaTime, AZ::ScriptTimePoint time) + { + m_appFrequencies.push_back(1.0f / deltaTime); + if (m_appFrequencies.size() > m_historySize) + { + m_appFrequencies.erase(m_appFrequencies.begin()); + } + } } // namespace SensorDebug diff --git a/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.h b/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.h index 3190390b..58b59427 100644 --- a/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.h +++ b/Gems/SensorDebug/Code/Source/Clients/SensorDebugSystemComponent.h @@ -4,17 +4,24 @@ #include #include +#include +#include #include +#include #include #include #include #include #include + namespace SensorDebug { + + class SensorDebugSystemComponent : public AZ::Component , public ImGui::ImGuiUpdateListenerBus::Handler + , private AZ::TickBus::Handler { public: AZ_COMPONENT_DECL(SensorDebugSystemComponent); @@ -35,12 +42,26 @@ namespace SensorDebug void ClearSensors(); private: + // AZ::TickBus::Handler interface implementation + void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; + + void UpdatePhysXConfig(); + void GetPhysXConfig(); + void Pause(bool isPaused); + AzPhysics::Scene* GetScene(); + void FindSensorsWithBusAPI(); void FindSensorsWithComponentAPI(); void FindSensorsWithGivenType(const char* typeId); AZStd::vector m_sensorEntities; + AZStd::unordered_map> m_sensorFrequencyHistory; AZStd::unordered_map m_sensorNames; AZStd::unordered_map m_entitiesNames; AZStd::unordered_map m_sensorFrequencies; + AZStd::vector m_appFrequencies; + float m_maxFPS = 60.0f; + int m_historySize = 1000; + AzPhysics::Scene* m_scene = nullptr; + PhysX::PhysXSystemConfiguration m_modifiedPhysXConfig; }; } // namespace SensorDebug diff --git a/Gems/SensorDebug/Code/Source/Tools/SensorDebugEditorSystemComponent.cpp b/Gems/SensorDebug/Code/Source/Tools/SensorDebugEditorSystemComponent.cpp index d3b257b1..631270fa 100644 --- a/Gems/SensorDebug/Code/Source/Tools/SensorDebugEditorSystemComponent.cpp +++ b/Gems/SensorDebug/Code/Source/Tools/SensorDebugEditorSystemComponent.cpp @@ -19,23 +19,25 @@ namespace SensorDebug void SensorDebugEditorSystemComponent::Activate() { - SensorDebugSystemComponent::Activate(); + AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusConnect(); } void SensorDebugEditorSystemComponent::Deactivate() { AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusDisconnect(); - SensorDebugSystemComponent::Deactivate(); + } void SensorDebugEditorSystemComponent::OnStartPlayInEditorBegin() { ClearSensors(); + SensorDebugSystemComponent::Activate(); } void SensorDebugEditorSystemComponent::OnStopPlayInEditor() { ClearSensors(); + SensorDebugSystemComponent::Deactivate(); } } // namespace SensorDebug