From 642de3a406f9d3895a27ed2590be6fea109dfb3c Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Thu, 19 Oct 2023 03:32:14 +0500 Subject: [PATCH] Implemented #217 --- ambf_framework/afFramework.cpp | 14 ++++++-------- ambf_framework/afFramework.h | 19 ++++++++----------- .../core/ros_comm_plugin/WorldCommPlugin.cpp | 3 ++- ambf_ros_modules/ambf_msgs/msg/WorldState.msg | 1 + .../ambf_server/include/ambf_server/World.h | 3 ++- ambf_simulator/src/ambf_simulator.cpp | 9 ++++----- 6 files changed, 23 insertions(+), 26 deletions(-) diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index 4f8be9ca8..00f15f6b4 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -1,4 +1,4 @@ -//============================================================================== +//============================================================================== /* Software License Agreement (BSD License) Copyright (c) 2019-2021, AMBF @@ -5938,7 +5938,7 @@ double afWorld::getSimulationDeltaTime() /// \param a_loopFreq /// \param a_numDevices /// -void afWorld::updateDynamics(double a_interval, double a_wallClock, double a_loopFreq, int a_numDevices) +void afWorld::updateDynamics(double a_interval, int a_numDevices) { // sanity check if (a_interval <= 0) { return; } @@ -5962,11 +5962,9 @@ void afWorld::updateDynamics(double a_interval, double a_wallClock, double a_loo } } - m_physicsFreq = a_loopFreq; + m_freqCounterPhysics.signal(1); m_numDevices = a_numDevices; - m_wallClock = a_wallClock; - double dt = getSimulationDeltaTime(); // integrate simulation during an certain interval @@ -6136,7 +6134,7 @@ void afWorld::removeSceneObjectFromWorld(cGenericObject *a_cObject) /// \return /// double afWorld::computeStepSize(bool adjust_intetration_steps){ - double step_size = g_wallClock.getCurrentTimeSeconds() - getSimulationTime(); + double step_size = m_wallClock.getCurrentTimeSeconds() - getSimulationTime(); if (adjust_intetration_steps){ int min_iterations = 2; if (step_size >= getIntegrationTimeStep() * min_iterations){ @@ -7309,11 +7307,11 @@ void afCamera::updateLabels(afRenderOptions &options) // We should prioritize the update of freqeunt labels // update haptic and graphic rate data - string wallTimeStr = "Wall Time: " + cStr(m_afWorld->g_wallClock.getCurrentTimeSeconds(), 2) + " s"; + string wallTimeStr = "Wall Time: " + cStr(m_afWorld->m_wallClock.getCurrentTimeSeconds(), 2) + " s"; string simTimeStr = "Sim Time: " + cStr(m_afWorld->getSimulationTime(), 2) + " s"; string graphicsFreqStr = "Gfx (" + cStr(m_afWorld->m_freqCounterGraphics.getFrequency(), 0) + " Hz)"; - string hapticFreqStr = "Phx (" + cStr(m_afWorld->m_freqCounterHaptics.getFrequency(), 0) + " Hz)"; + string hapticFreqStr = "Phx (" + cStr(m_afWorld->m_freqCounterPhysics.getFrequency(), 0) + " Hz)"; string timeLabelStr = wallTimeStr + " / " + simTimeStr; string dynHapticFreqLabelStr = graphicsFreqStr + " / " + hapticFreqStr; diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index 264c39891..af00cb574 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -2203,7 +2203,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ int getMaxIterations(){return m_maxIterations;} // This method returns the current simulation time - double getWallTime(){return m_wallClock;} + double getWallTime(){return m_wallClock.getCurrentTimeSeconds();} // This method returns the current simulation time double getSimulationTime(){return m_simulationTime;} @@ -2218,7 +2218,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ void estimateBodyWrenches(); //! This method updates the simulation over a time interval. - virtual void updateDynamics(double a_interval, double a_wallClock=0, double a_loopFreq = 0, int a_numDevices = 0); + virtual void updateDynamics(double a_interval, int a_numDevices = 0); //! This method updates the position and orientation from Bullet models to CHAI3D models. virtual void updateSceneObjects(); @@ -2251,7 +2251,9 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ bool isHeadless(); - int getPhysicsFrequency(){return m_physicsFreq;} + int getPhysicsFrequency(){return m_freqCounterPhysics.getFrequency();} + + int getGraphicsFrequency(){return m_freqCounterGraphics.getFrequency();} int getNumDevices(){return m_numDevices;} @@ -2306,7 +2308,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ cMesh* m_pickSphere = nullptr; - cPrecisionClock g_wallClock; + cPrecisionClock m_wallClock; // Vertex Shader Filepath afPath m_vsFilePath; @@ -2330,7 +2332,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ cFrequencyCounter m_freqCounterGraphics; // a frequency counter to measure the simulation haptic rate - cFrequencyCounter m_freqCounterHaptics; + cFrequencyCounter m_freqCounterPhysics; map m_pcMap; @@ -2371,10 +2373,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ // Integration time step. double m_integrationTimeStep; - // Wall Clock in Secs - double m_wallClock; - - // Last Simulation Time + // Last Simulation Timebn double m_lastSimulationTime; // Maximum number of iterations. @@ -2382,8 +2381,6 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ afWorldPluginManager m_pluginManager; - int m_physicsFreq = 0; - int m_numDevices = 0; private: diff --git a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp index f369d3a19..de5762b64 100644 --- a/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/WorldCommPlugin.cpp @@ -107,7 +107,8 @@ void afWorldCommunicationPlugin::worldUpdateState(afWorldPtr worldPtr, double dt m_afWorldCommPtr->set_wall_time(worldPtr->getWallTime()); m_afWorldCommPtr->set_sim_time(worldPtr->getSimulationTime()); m_afWorldCommPtr->set_time_stamp(worldPtr->getCurrentTimeStamp()); - m_afWorldCommPtr->set_loop_freq(worldPtr->getPhysicsFrequency()); + m_afWorldCommPtr->set_graphics_loop_freq(worldPtr->getGraphicsFrequency()); + m_afWorldCommPtr->set_physics_loop_freq(worldPtr->getPhysicsFrequency()); m_afWorldCommPtr->set_num_devices(worldPtr->getNumDevices()); } diff --git a/ambf_ros_modules/ambf_msgs/msg/WorldState.msg b/ambf_ros_modules/ambf_msgs/msg/WorldState.msg index 598dcd5f5..dbaa55cda 100644 --- a/ambf_ros_modules/ambf_msgs/msg/WorldState.msg +++ b/ambf_ros_modules/ambf_msgs/msg/WorldState.msg @@ -1,4 +1,5 @@ Header header +uint64 graphics_loop_freq uint64 dynamic_loop_freq uint8 n_devices uint32 sim_step diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/World.h b/ambf_ros_modules/ambf_server/include/ambf_server/World.h index 387eccdcc..e8687cb67 100644 --- a/ambf_ros_modules/ambf_server/include/ambf_server/World.h +++ b/ambf_ros_modules/ambf_server/include/ambf_server/World.h @@ -126,7 +126,8 @@ class World: public WorldRosCom, public WorldParams{ public: World(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); inline void set_num_devices(uint a_num){m_State.n_devices = a_num;} - inline void set_loop_freq(double a_freq){m_State.dynamic_loop_freq = a_freq;} + inline void set_graphics_loop_freq(double a_freq){m_State.graphics_loop_freq = a_freq;} + inline void set_physics_loop_freq(double a_freq){m_State.dynamic_loop_freq = a_freq;} virtual void increment_sim_step(); inline bool step_sim(){return m_stepSim;} diff --git a/ambf_simulator/src/ambf_simulator.cpp b/ambf_simulator/src/ambf_simulator.cpp index 638a69cae..1b235d177 100644 --- a/ambf_simulator/src/ambf_simulator.cpp +++ b/ambf_simulator/src/ambf_simulator.cpp @@ -548,7 +548,7 @@ int main(int argc, char* argv[]) } else{ - std::cerr << "\nRunning Headless (-g option provided) t = " << g_afWorld->g_wallClock.getCurrentTimeSeconds() << " sec" << std::endl; + std::cerr << "\nRunning Headless (-g option provided) t = " << g_afWorld->m_wallClock.getCurrentTimeSeconds() << " sec" << std::endl; sleep(1.0); } graphicsRate.sleep(); @@ -598,7 +598,7 @@ void updatePhysics(){ g_simulationFinished = false; // start haptic device - g_afWorld->g_wallClock.start(true); + g_afWorld->m_wallClock.start(true); afRate phxSleep(g_cmdOpts.phxFrequency); bool bodyPicked = false; @@ -620,7 +620,6 @@ void updatePhysics(){ g_afWorld->resetDynamicBodies(); g_bodiesResetFlag = false; } - g_afWorld->m_freqCounterHaptics.signal(1); // Take care of any picked body by mouse if (g_pickBody){ @@ -723,7 +722,7 @@ void updatePhysics(){ simDev->P_ac_ramp = 1.0; } } - g_afWorld->updateDynamics(step_size, g_afWorld->g_wallClock.getCurrentTimeSeconds(), g_afWorld->m_freqCounterHaptics.getFrequency(), g_inputDevices->m_numDevices); + g_afWorld->updateDynamics(step_size, g_inputDevices->m_numDevices); if (!g_afWorld->isPhysicsPaused()){ g_pluginManager.physicsUpdate(step_size); } @@ -875,7 +874,7 @@ void updateHapticDevice(void* ccuPtr){ } - if (g_afWorld->g_wallClock.getCurrentTimeSeconds() < wait_time){ + if (g_afWorld->m_wallClock.getCurrentTimeSeconds() < wait_time){ phyDev->setPosClutched(phyDev->getPos()); }