From ae1f030a0a5796a99bfe5c5ce3e4f85ba224ff8a Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Thu, 1 Aug 2019 20:38:40 +0800 Subject: [PATCH] 1. MPI_Allgatherv, version 0.6.1 2. in order to reduce time cost of UpdateCollision and UpdatePhysics, make the robots not simulated in local gazebo that is static. 3. modify the loadPlugins code, use ownModels list --- .../gazebo/physics/World.cc | 103 +++--------------- 1 file changed, 18 insertions(+), 85 deletions(-) diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index 51fcc5d..b4c8f0d 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -1362,7 +1362,8 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf, BasePtr _parent) { if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(j)) { - this->dataPtr->ownModels.push_back(model); + this->dataPtr->ownModels.push_back(model); // add model to ownModels list + this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); // to maintain model ID list in GazeboID } } } @@ -1373,7 +1374,8 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf, BasePtr _parent) { if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(j)) { - model->SetStatic(true); + model->SetStatic(true); // model that not simulated in local gazebo is set to be static to reduce computation in UpdateCollision and UpdatePhysics + this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); // to maintain model ID list in GazeboID } } } @@ -1914,58 +1916,24 @@ void World::LoadPlugins() ModelPtr model = boost::static_pointer_cast( this->dataPtr->rootElement->GetChild(i)); - // Modified by zhangshuai 2019.04.02 ----Begin + // Modified by zhangshuai 2019.07.30 ----Begin if (this->flag == 1) { // Judge if the model to load is simulated in this gazebo - - // for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) - // { - // if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) - // { - // // Original part - // model->LoadPlugins(); - // // Original part - // this->distribution->GetGazeboIDPtr(gazeboLocalID)->SetModelID(i, model->GetId()); - // } - // } - - int tmp_gazebo_count = this->distribution->GetGazeboCount(); - for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++) + for (unsigned int i = 0; i < this->GetOwnModelCount(); i++) { - if (this->gazeboLocalID == tmp_gazebo_id) - { - unsigned int tmp_model_count = this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); - for (unsigned int j = 0; j < tmp_model_count; j++) - { - if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(j)) - { - // Original part - model->LoadPlugins(); - // Original part - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); - } - } - } - else - { - unsigned int tmp_model_count = this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); - for (unsigned int j = 0; j < tmp_model_count; j++) - { - if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(j)) - { - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); - } - } - } + if (model->GetName() == this->GetOwnModel(i)->GetName()) + model->LoadPlugins(); } } else { + // Original part model->LoadPlugins(); + // Original part } + // Modified by zhangshuai 2019.07.30 ----End - // Modified by zhangshuai 2019.04.02 ----End // model->LoadPlugins(); } } @@ -2479,58 +2447,23 @@ void World::ProcessFactoryMsgs() if (model != nullptr) { model->Init(); - // Modified by zhangshuai 2019.04.02 ----Begin + // Modified by zhangshuai 2019.07.30 ----Begin if (this->flag == 1) { // Judge if the model to load is simulated in this gazebo - - // for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) - // { - // if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) - // { - // // Original part - // model->LoadPlugins(); - // // Original part - // this->distribution->GetGazeboIDPtr(gazeboLocalID)->SetModelID(i, model->GetId()); - // } - // } - - int tmp_gazebo_count = this->distribution->GetGazeboCount(); - for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++) + for (unsigned int i = 0; i < this->GetOwnModelCount(); i++) { - if (this->gazeboLocalID == tmp_gazebo_id) - { - unsigned int tmp_model_count = this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); - for (unsigned int i = 0; i < tmp_model_count; i++) - { - if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(i)) - { - // Original part - model->LoadPlugins(); - // Original part - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(i, model->GetId()); - } - } - } - else - { - unsigned int tmp_model_count = this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); - for (unsigned int i = 0; i < tmp_model_count; i++) - { - if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(i)) - { - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(i, model->GetId()); - } - } - } + if (model->GetName() == this->GetOwnModel(i)->GetName()) + model->LoadPlugins(); } } else { + // Original part model->LoadPlugins(); + // Original part } - - // Modified by zhangshuai 2019.04.02 ----End + // Modified by zhangshuai 2019.07.30 ----End } } catch (...)