From 648752d2a676610418e50594e0eee28a8b5575da Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Wed, 23 Oct 2019 16:16:26 +0800 Subject: [PATCH] 1. MPI_Allgatherv, version 0.6.3 2. modify printing results in world.cc --- .../gazebo/physics/World.cc | 105 ++++++++++++++---- 1 file changed, 83 insertions(+), 22 deletions(-) diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index a36002c..0c9f923 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -86,6 +86,7 @@ #include "gazebo/physics/Population.hh" #include "gazebo/countTime.hh" // Added by zhangshuai 2019.04.03 for count time +// #include // Added by zhangshuai 2019.10.23 for print results using namespace gazebo; using namespace physics; @@ -96,14 +97,15 @@ bool g_clearModels; // Added by zhangshuai 2019.04.03 for count time ----Begin #ifdef USE_COUNT_TIME -double before_worldUpdateBeginTime = 0; +double before_worldUpdateBeginTime = 0; // little double worldUpdateBeginTime = 0; -double modelUpdateFuncTime = 0; +double modelUpdateFuncTime = 0; // little double updateCollisionTime = 0; -double loggingTime = 0; -double before_updatePhysicsTime = 0; +double loggingTime = 0; // little +double before_updatePhysicsTime = 0; // little double updatePhysicsTime = 0; double before_mpiTime = 0; +double barrierTime = 0; double mpiTime = 0; double after_mpiTime = 0; double wholeUpdataTime = 0; @@ -116,9 +118,11 @@ double loggingTime2 = 0; double before_updatePhysicsTime2 = 0; double updatePhysicsTime2 = 0; double before_mpiTime2 = 0; +double barrierTime2 = 0; double mpiTime2 = 0; double after_mpiTime2 = 0; double wholeUpdataTime2 = 0; + double simtime = 0; double realtime = 0; uint64_t start_iterations = 0; @@ -988,8 +992,20 @@ void World::Update() #endif // Added by zhangshuai 2019.04.03 for count time ----End + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + MPI_Barrier(MPI_COMM_WORLD); + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + barrierTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; +#endif + // Added by zhangshuai 2019.04.03 for count time ----Begin #ifdef USE_COUNT_TIME gettimeofday(&tv, NULL); @@ -1068,7 +1084,7 @@ void World::Update() #ifdef USE_COUNT_TIME // std::cout << "===== end iterations:\t" << this->dataPtr->iterations << std::endl; - if (this->dataPtr->iterations == 0) + if (this->dataPtr->iterations == 1) { before_worldUpdateBeginTime2 = before_worldUpdateBeginTime; worldUpdateBeginTime2 = worldUpdateBeginTime; @@ -1078,6 +1094,7 @@ void World::Update() before_updatePhysicsTime2 = before_updatePhysicsTime; updatePhysicsTime2 = updatePhysicsTime; before_mpiTime2 = before_mpiTime; + barrierTime2 = barrierTime; mpiTime2 = mpiTime; after_mpiTime2 = after_mpiTime; wholeUpdataTime2 = wholeUpdataTime; @@ -1086,26 +1103,70 @@ void World::Update() start_iterations = this->dataPtr->iterations; } - if (this->dataPtr->iterations == 100000) + if (this->dataPtr->iterations == 100001) { // if (this->gazeboLocalID == 0) // { - std::cout << "===== average before_worldUpdateBeginTime:\t" << (before_worldUpdateBeginTime - before_worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average worldUpdateBeginTime:\t" << (worldUpdateBeginTime - worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average modelUpdateFuncTime:\t" << (modelUpdateFuncTime - modelUpdateFuncTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average updateCollisionTime:\t" << (updateCollisionTime - updateCollisionTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average loggingTime:\t" << (loggingTime - loggingTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average before_updatePhysicsTime:\t" << (before_updatePhysicsTime - before_updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average updatePhysicsTime:\t" << (updatePhysicsTime - updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average before_mpiTime:\t" << (before_mpiTime - before_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average mpiTime:\t" << (mpiTime - mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average after_mpiTime:\t" << (after_mpiTime - after_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - double wholeMpiTime = before_mpiTime + mpiTime + after_mpiTime - (before_mpiTime2 + mpiTime2 + after_mpiTime2); - std::cout << "===== average wholeMpiTime:\t" << wholeMpiTime * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_mpiTime + mpiTime + after_mpiTime - (before_worldUpdateBeginTime2 + worldUpdateBeginTime2 + modelUpdateFuncTime2 + updateCollisionTime2 + loggingTime2 + before_updatePhysicsTime2 + updatePhysicsTime2 + before_mpiTime2 + mpiTime2 + after_mpiTime2); - std::cout << "===== average wholeAddTime:\t" << wholeAddTime * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average wholeUpdateTime:\t" << (wholeUpdataTime - wholeUpdataTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; - std::cout << "===== average RTF:\t" << (this->GetSimTime().Double() - simtime) / (this->GetRealTime().Double() - realtime) << "\t =====\t" << this->gazeboLocalID << std::endl; + double ave_RTF = (this->GetSimTime().Double() - simtime) / (this->GetRealTime().Double() - realtime); + double ave_wholeStepTime = 1.0 / ave_RTF; + // double ave_wholeUpdateTime = (wholeUpdataTime - wholeUpdataTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_worldUpdateBeginTime = (worldUpdateBeginTime - worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_updateCollisionTime = (updateCollisionTime - updateCollisionTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_updatePhysicsTime = (updatePhysicsTime - updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_barrierTime = (barrierTime - barrierTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_before_mpiTime = (before_mpiTime - before_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_mpiTime = (mpiTime - mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_after_mpiTime = (after_mpiTime - after_mpiTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations); + double ave_wholeMpiTime = ave_before_mpiTime + ave_mpiTime + ave_after_mpiTime; + double ave_othersTime = ave_wholeStepTime - ave_wholeMpiTime - ave_barrierTime - ave_updatePhysicsTime - ave_updateCollisionTime - ave_worldUpdateBeginTime; + + double perce_worldUpdateBeginTime = (ave_worldUpdateBeginTime / ave_wholeStepTime) * 100; + double perce_updateCollisionTime = (ave_updateCollisionTime / ave_wholeStepTime) * 100; + double perce_updatePhysicsTime = (ave_updatePhysicsTime / ave_wholeStepTime) * 100; + double perce_barrierTime = (ave_barrierTime / ave_wholeStepTime) * 100; + double perce_wholeMpiTime = (ave_wholeMpiTime / ave_wholeStepTime) * 100; + double perce_othersTime = (ave_othersTime / ave_wholeStepTime) * 100; + + double perce_before_mpiTime = (ave_before_mpiTime / ave_wholeMpiTime) * 100; + double perce_mpiTime = (ave_mpiTime / ave_wholeMpiTime) * 100; + double perce_after_mpiTime = (ave_after_mpiTime / ave_wholeMpiTime) * 100; + + if (this->gazeboLocalID != 0) + gazebo::common::Time::MSleep((unsigned int)this->gazeboLocalID * 100); + + std::cout << "***************************************************************************" << std::endl; + std::cout << "===== average worldUpdateBeginTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_worldUpdateBeginTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_worldUpdateBeginTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average updateCollisionTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_updateCollisionTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_updateCollisionTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average updatePhysicsTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_updatePhysicsTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_updatePhysicsTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average barrierTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_barrierTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_barrierTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average wholeMpiTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_wholeMpiTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_wholeMpiTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average othersTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_othersTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_othersTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average wholeStepTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_wholeStepTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << 100.0 << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average RTF:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(3) << ave_RTF << "\t =====\t" << this->gazeboLocalID << std::endl; + + std::cout << "---------------------------------------------------------------------------" << std::endl; + std::cout << "===== average before_mpiTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_before_mpiTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_before_mpiTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average mpiTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_mpiTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_mpiTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "===== average after_mpiTime:\t" << std::setiosflags(std::ios::fixed) << std::setprecision(4) << ave_after_mpiTime << " (" << std::setiosflags(std::ios::fixed) << std::setprecision(2) << perce_after_mpiTime << "%)" + << "\tms =====\t" << this->gazeboLocalID << std::endl; + std::cout << "***************************************************************************" << std::endl; + + // std::cout << "===== average before_worldUpdateBeginTime:\t" << (before_worldUpdateBeginTime - before_worldUpdateBeginTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + // std::cout << "===== average modelUpdateFuncTime:\t" << (modelUpdateFuncTime - modelUpdateFuncTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + // std::cout << "===== average loggingTime:\t" << (loggingTime - loggingTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + // std::cout << "===== average before_updatePhysicsTime:\t" << (before_updatePhysicsTime - before_updatePhysicsTime2) * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + // double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_mpiTime + mpiTime + after_mpiTime - (before_worldUpdateBeginTime2 + worldUpdateBeginTime2 + modelUpdateFuncTime2 + updateCollisionTime2 + loggingTime2 + before_updatePhysicsTime2 + updatePhysicsTime2 + before_mpiTime2 + mpiTime2 + after_mpiTime2); + // std::cout << "===== average wholeAddTime:\t" << wholeAddTime * 1000.0 / (this->dataPtr->iterations - start_iterations) << "\tms =====\t" << this->gazeboLocalID << std::endl; + // } } #endif