diff --git a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc index 830ba6f..2176540 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.cc @@ -79,27 +79,13 @@ void GazeboID::Load(sdf::ElementPtr _sdf) if (this->sdf->HasAttribute("num")) this->gazeboID = this->sdf->Get("num"); - if (this->sdf->HasAttribute("ip")) - this->ip = this->sdf->Get("ip"); - - // if (this->sdf->HasElement("ip")) - // { - // sdf::ElementPtr gazeboIdElem = this->sdf->GetElement("ip"); - // ip = gazeboIdElem->Get(); - // } - - // if (this->sdf->HasElement("port")) - // { - // sdf::ElementPtr gazeboPortElem = this->sdf->GetElement("port"); - // port = gazeboPortElem->Get(); - // } - if (this->sdf->HasElement("model_name")) { sdf::ElementPtr modelNameElem = this->sdf->GetElement("model_name"); while (modelNameElem) { modelNames.push_back(modelNameElem->Get()); + modelIDs.push_back(0); //initial state is 0 modelNameElem = modelNameElem->GetNextElement("model_name"); } } @@ -111,12 +97,6 @@ int GazeboID::GetGazeboID() return gazeboID; } -////////////////////////////////////////////////// -std::string GazeboID::GetGazeboIp() -{ - return ip; -} - ////////////////////////////////////////////////// unsigned int GazeboID::GetModelCount() { @@ -129,6 +109,18 @@ std::string GazeboID::GetModelName(unsigned int _i) return modelNames[_i]; } +////////////////////////////////////////////////// +unsigned int GazeboID::GetModelID(unsigned int _i) +{ + return modelIDs[_i]; +} + +////////////////////////////////////////////////// +void GazeboID::SetModelID(unsigned int _i, unsigned int _id) +{ + modelIDs[_i] = _id; +} + ////////////////////////////////////////////////// void GazeboID::Fini() { diff --git a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh index fdab0ee..e96b390 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh +++ b/Gazebo_Distributed_MPI/gazebo/physics/GazeboID.hh @@ -63,11 +63,6 @@ public: public: int GetGazeboID(); - /// \brief Get the gazebo ip of this Distribution. - /// \return the gazebo ip of this Distribution. -public: - std::string GetGazeboIp(); - /// \brief Get the number of models this Distribution has. /// \return Number of models associated with this Distribution. public: @@ -79,6 +74,18 @@ public: public: std::string GetModelName(unsigned int _i); + /// \brief Get the ID of the specified model. + /// \param[in] _i Number of the model ID to get. + /// \return the ID of the specified model. +public: + unsigned int GetModelID(unsigned int _i); + + /// \brief Set the ID of the specified model. + /// \param[in] _i Number of the model to set ID. + /// \param[in] _id ID of the model to set. +public: + void SetModelID(unsigned int _i, unsigned int _id); + /// \brief Finialize the gazeboID public: void Fini(); @@ -91,17 +98,13 @@ private: private: int gazeboID; - /// \brief ip of distributed gazebo. -private: - std::string ip; - - /// \brief port of distributed gazebo. -private: - int port; - /// \brief Models name in this gazebo private: std::vector modelNames; + + /// \brief Models ID in this gazebo +private: + std::vector modelIDs; }; } // namespace physics diff --git a/Gazebo_Distributed_MPI/gazebo/physics/MpiCommunication.hh b/Gazebo_Distributed_MPI/gazebo/physics/MpiCommunication.hh index ac14cd3..6f1377a 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/MpiCommunication.hh +++ b/Gazebo_Distributed_MPI/gazebo/physics/MpiCommunication.hh @@ -29,18 +29,15 @@ #include "gazebo/util/system.hh" #include "gazebo/math/Pose.hh" -#define QUEUE 1 -#define BUF_SIZE 8192 - namespace gazebo { namespace physics { -/// \brief Structure of Pose information communicated with Tcp +/// \brief Structure of Pose information communicated with MPI struct CommunicationData { - char model_name[16]; + // char model_name[16]; math::Pose model_pose; }; diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index 5edfa7e..429ec3b 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -951,15 +951,8 @@ void World::Update() CommunicationData *sendBufferData = new CommunicationData[tmp_local_model_count]; for (unsigned int i = 0; i < tmp_local_model_count; i++) { - std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i); - unsigned int j = 0; - - for (j = 0; j < tmp_model_name.length(); j++) - sendBufferData[i].model_name[j] = tmp_model_name[j]; - sendBufferData[i].model_name[j] = '\0'; - sendBufferData[i].model_pose = this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose(); - - tmp_model_name.clear(); + // sendBufferData[i].model_pose = this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose(); + sendBufferData[i].model_pose = this->GetModelById(this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelID(i))->GetWorldPose(); } sendBuffer = (char *)sendBufferData; @@ -1010,9 +1003,8 @@ void World::Update() for (unsigned int i = 0; i < tmp_model_count; i++) { - std::string tmp_model_name = tmpBufferData[i].model_name; - this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmpBufferData[i].model_pose); - tmp_model_name.clear(); + // this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmpBufferData[i].model_pose); + this->GetModelById(this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelID(i))->SetWorldPose(tmpBufferData[i].model_pose); } delete[] tmpBufferData; } @@ -2385,13 +2377,45 @@ void World::ProcessFactoryMsgs() 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++) + + // 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++) { - if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) + if (this->gazeboLocalID == tmp_gazebo_id) { - // Original part - model->LoadPlugins(); - // Original part + 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()); + } + } } } } diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo_mpi_spawn_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo_mpi_spawn_test.launch new file mode 100644 index 0000000..fd59b83 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_100_single_gazebo_mpi_spawn_test.launch @@ -0,0 +1,1108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_40_single_gazebo_mpi_spawn_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_40_single_gazebo_mpi_spawn_test.launch new file mode 100644 index 0000000..4e6c8de --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_40_single_gazebo_mpi_spawn_test.launch @@ -0,0 +1,448 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_50_single_gazebo_mpi_spawn_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_50_single_gazebo_mpi_spawn_test.launch new file mode 100644 index 0000000..c0e84ef --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_50_single_gazebo_mpi_spawn_test.launch @@ -0,0 +1,558 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test.launch b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test.launch index 726204e..369a8a5 100644 --- a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test.launch +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_60_single_gazebo_mpi_spawn_test.launch @@ -445,7 +445,7 @@ - + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_100hector_mpi_test.world b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_100hector_mpi_test.world new file mode 100644 index 0000000..203a8ec --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_100hector_mpi_test.world @@ -0,0 +1,385 @@ + + + + + + model://sun + + + + model://kunming_airport + 0 0 0 0 0 0 + + + + 0.68 0.68 0.68 1.0 + + + + 0 + + + + + + + + bebop_0 + bebop_1 + bebop_2 + bebop_3 + bebop_4 + bebop_5 + bebop_6 + bebop_7 + bebop_8 + bebop_9 + bebop_10 + bebop_11 + bebop_12 + bebop_13 + bebop_14 + bebop_15 + bebop_16 + bebop_17 + bebop_18 + bebop_19 + bebop_20 + bebop_21 + bebop_22 + bebop_23 + bebop_24 + bebop_25 + bebop_26 + bebop_27 + bebop_28 + bebop_29 + bebop_30 + bebop_31 + bebop_32 + bebop_33 + bebop_34 + bebop_35 + bebop_36 + bebop_37 + bebop_38 + bebop_39 + bebop_40 + bebop_41 + bebop_42 + bebop_43 + bebop_44 + bebop_45 + bebop_46 + bebop_47 + bebop_48 + bebop_49 + + + bebop_50 + bebop_51 + bebop_52 + bebop_53 + bebop_54 + bebop_55 + bebop_56 + bebop_57 + bebop_58 + bebop_59 + bebop_60 + bebop_61 + bebop_62 + bebop_63 + bebop_64 + bebop_65 + bebop_66 + bebop_67 + bebop_68 + bebop_69 + bebop_70 + bebop_71 + bebop_72 + bebop_73 + bebop_74 + bebop_75 + bebop_76 + bebop_77 + bebop_78 + bebop_79 + bebop_80 + bebop_81 + bebop_82 + bebop_83 + bebop_84 + bebop_85 + bebop_86 + bebop_87 + bebop_88 + bebop_89 + bebop_90 + bebop_91 + bebop_92 + bebop_93 + bebop_94 + bebop_95 + bebop_96 + bebop_97 + bebop_98 + bebop_99 + + + + + 0 + 0.001 + 0 + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_40hector_mpi_test.world b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_40hector_mpi_test.world new file mode 100644 index 0000000..9492fb8 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_40hector_mpi_test.world @@ -0,0 +1,325 @@ + + + + + + model://sun + + + + model://kunming_airport + 0 0 0 0 0 0 + + + + 0.68 0.68 0.68 1.0 + + + + 0 + + + + + + + + bebop_0 + bebop_1 + bebop_2 + bebop_3 + bebop_4 + bebop_5 + bebop_6 + bebop_7 + bebop_8 + bebop_9 + bebop_10 + bebop_11 + bebop_12 + bebop_13 + bebop_14 + bebop_15 + bebop_16 + bebop_17 + bebop_18 + bebop_19 + + + bebop_20 + bebop_21 + bebop_22 + bebop_23 + bebop_24 + bebop_25 + bebop_26 + bebop_27 + bebop_28 + bebop_29 + bebop_30 + bebop_31 + bebop_32 + bebop_33 + bebop_34 + bebop_35 + bebop_36 + bebop_37 + bebop_38 + bebop_39 + + + + + 0 + 0.001 + 0 + + + + + + + diff --git a/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_50hector_mpi_test.world b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_50hector_mpi_test.world new file mode 100644 index 0000000..baedfa3 --- /dev/null +++ b/Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_gazebo/worlds/kunming_airport_distribution_50hector_mpi_test.world @@ -0,0 +1,335 @@ + + + + + + model://sun + + + + model://kunming_airport + 0 0 0 0 0 0 + + + + 0.68 0.68 0.68 1.0 + + + + 0 + + + + + + + + bebop_0 + bebop_1 + bebop_2 + bebop_3 + bebop_4 + bebop_5 + bebop_6 + bebop_7 + bebop_8 + bebop_9 + bebop_10 + bebop_11 + bebop_12 + bebop_13 + bebop_14 + bebop_15 + bebop_16 + bebop_17 + bebop_18 + bebop_19 + bebop_20 + bebop_21 + bebop_22 + bebop_23 + bebop_24 + + + bebop_25 + bebop_26 + bebop_27 + bebop_28 + bebop_29 + bebop_30 + bebop_31 + bebop_32 + bebop_33 + bebop_34 + bebop_35 + bebop_36 + bebop_37 + bebop_38 + bebop_39 + bebop_40 + bebop_41 + bebop_42 + bebop_43 + bebop_44 + bebop_45 + bebop_46 + bebop_47 + bebop_48 + bebop_49 + + + + + 0 + 0.001 + 0 + + + + + + +