From aa80c5b61be1ecfe62721fd2611ccb5a4b4d0894 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Thu, 11 Jul 2019 17:16:11 +0800 Subject: [PATCH] 1. MPI_Allgatherv, version 0.5.2 2. add "ownModels" list and some functions in world files, for MPI, "ownModels" are used for modelUpdteFunc 3. delete "models" list in GazeboID 4. add launch file that 30 hector takeoff 20 hector do not move for testing collision time --- .../gazebo/physics/GazeboID.cc | 13 - .../gazebo/physics/GazeboID.hh | 12 - .../gazebo/physics/World.cc | 43 +- .../gazebo/physics/World.hh | 16 + .../gazebo/physics/WorldPrivate.hh | 5 + .../mpi_run/MPI_Gazebo_launch.sh | 6 +- ...gle_gazebo_mpi_spawn_test_20_static.launch | 558 ++++++++++++++++++ 7 files changed, 618 insertions(+), 35 deletions(-) create mode 100644 Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_30_single_gazebo_mpi_spawn_test_20_static.launch diff --git a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc index e83692a..0e1f2fa 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc @@ -127,16 +127,3 @@ void GazeboID::Fini() this->modelNames.clear(); this->sdf.reset(); } - -////////////////////////////////////////////////// -ModelPtr GazeboID::GetModel(unsigned int _index) const -{ - if (_index >= this->models.size()) - { - gzerr << "Given model index[" << _index << "] is out of range[0.." - << this->models.size() << "]\n"; - return ModelPtr(); - } - - return this->models[_index]; -} \ No newline at end of file diff --git a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh index cfc4fce..df630b8 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh +++ b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh @@ -89,14 +89,6 @@ public: /// \brief Finialize the gazeboID public: void Fini(); - - /// \brief Get a model based on an index. - /// Get a Model using an index, where index must be greater than zero - /// and less than GazeboID::GetModelCount() - /// \param[in] _index The index of the model [0..GetModelCount) - /// \return A pointer to the Model. NULL if _index is invalid. -public: - ModelPtr GetModel(unsigned int _index) const; /// \brief The Distribution's current SDF description. private: @@ -113,10 +105,6 @@ private: /// \brief Models ID in this gazebo private: std::vector modelIDs; - - /// \brief A cached list of models. This is here for performance. -public: - Model_V models; }; } // namespace physics diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index c20b4a2..c98f4cf 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -1349,6 +1349,18 @@ ModelPtr World::LoadModel(sdf::ElementPtr _sdf, BasePtr _parent) this->PublishModelPose(model); this->dataPtr->models.push_back(model); + // Added by zhangshuai for MPI 2019.07.11 ----Begin + if (this->flag == 1) + { + for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) + { + if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) + { + this->dataPtr->ownModels.push_back(model); + } + } + } + // Added by zhangshuai for MPI 2019.07.11 ----End return model; } @@ -1846,9 +1858,9 @@ void World::ModelUpdateSingleLoop() // Modified by zhangshuai for MPI 2019.07.11 ----Begin if (this->flag == 1) { - for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) + for (unsigned int i = 0; i < this->GetOwnModelCount(); i++) { - this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModel(i)->Update(); + this->GetOwnModel(i)->Update(); } } else @@ -1913,7 +1925,6 @@ void World::LoadPlugins() model->LoadPlugins(); // Original part this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->models.push_back(model); } } } @@ -1925,7 +1936,6 @@ void World::LoadPlugins() if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(j)) { this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(j, model->GetId()); - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->models.push_back(model); } } } @@ -2480,7 +2490,6 @@ void World::ProcessFactoryMsgs() model->LoadPlugins(); // Original part this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(i, model->GetId()); - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->models.push_back(model); // modified by zhangshuai 2019.07.10 } } } @@ -2492,7 +2501,6 @@ void World::ProcessFactoryMsgs() if (model->GetName() == this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelName(i)) { this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->SetModelID(i, model->GetId()); - this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->models.push_back(model); // modified by zhangshuai 2019.07.10 } } } @@ -3430,4 +3438,25 @@ int World::GetFlag() { return this->flag; } -// Added by zhangshuai 2019.04.02 ----End \ No newline at end of file +// Added by zhangshuai 2019.04.02 ----End + +// Added by zhangshuai for MPI 2019.07.11 ----Begin +////////////////////////////////////////////////// +unsigned int World::GetOwnModelCount() const +{ + return this->dataPtr->ownModels.size(); +} + +////////////////////////////////////////////////// +ModelPtr World::GetOwnModel(unsigned int _index) const +{ + if (_index >= this->dataPtr->ownModels.size()) + { + gzerr << "Given model index[" << _index << "] is out of range[0.." + << this->dataPtr->models.size() << "]\n"; + return ModelPtr(); + } + + return this->dataPtr->ownModels[_index]; +} +// Added by zhangshuai for MPI 2019.07.11 ----End \ No newline at end of file diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.hh b/Gazebo_Distributed_MPI/gazebo/physics/World.hh index ef300e2..13e03f1 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.hh +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.hh @@ -776,6 +776,22 @@ private: // public: // int GetFlag(); // Added by zhangshuai 2019.06.10 ----End + + // Added by zhangshuai for MPI 2019.07.11 ----Begin + /// \brief Get the number of own models. + /// \return The number of own models in the World. +public: + unsigned int GetOwnModelCount() const; + + /// \brief Get a model based on an index. + /// Get a Model using an index, where index must be greater than zero + /// and less than World::GetOwnModelCount() + /// \param[in] _index The index of the model [0..GetOwnModelCount) + /// \return A pointer to the Model. NULL if _index is invalid. +public: + ModelPtr GetOwnModel(unsigned int _index) const; + // Added by zhangshuai for MPI 2019.07.11 ----End + }; /// \} } // namespace physics diff --git a/Gazebo_Distributed_MPI/gazebo/physics/WorldPrivate.hh b/Gazebo_Distributed_MPI/gazebo/physics/WorldPrivate.hh index 2f40626..7bb5ab2 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/WorldPrivate.hh +++ b/Gazebo_Distributed_MPI/gazebo/physics/WorldPrivate.hh @@ -336,6 +336,11 @@ namespace gazebo /// \brief Simulation time of the last log state captured. public: gazebo::common::Time logLastStateTime; + + // Added by zhangshuai for MPI 2019.07.11 ----Begin + /// \brief A cached list of models to simulated in local Gazebo for MPI. This is here for performance. + public: Model_V ownModels; + // Added by zhangshuai for MPI 2019.07.11 ----End }; } } diff --git a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh index 5a427d7..664ce9e 100755 --- a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh +++ b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh @@ -70,7 +70,7 @@ do echo "${hosts[$i]}:roslaunch_good" done -sleep 20s +sleep 40s #登入各节点开始仿真 for((i=0;i /dev/null 2>&1 "cd ~/Git/Gazebo_Distributed_MPI/mpi_run; diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_30_single_gazebo_mpi_spawn_test_20_static.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_30_single_gazebo_mpi_spawn_test_20_static.launch new file mode 100644 index 0000000..345e019 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_30_single_gazebo_mpi_spawn_test_20_static.launch @@ -0,0 +1,558 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +