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 (...)