Implemented WPI-AIM#217

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;

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<string, afPointCloudPtr> m_pcMap;
 
@@ -2371,19 +2373,14 @@ 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.
     int m_integrationMaxIterations;
 
     afWorldPluginManager m_pluginManager;
 
-    int m_physicsFreq = 0;
-
     int m_numDevices = 0;
 
 private:

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());
 }
 

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

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;}
 

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());
             }