diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index 4b61b19..5edfa7e 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -301,21 +301,14 @@ void World::Load(sdf::ElementPtr _sdf) // for MPI_Allgatherv this->sendBufferLen = sizeof(CommunicationData) * tmp_local_model_count; - this->receiveBufferLen = 0; + // this->receiveBufferLen = 0; this->modelCounts = 0; - this->bufferLen = new int[tmp_gazebo_count]; - this->displs = new int[tmp_gazebo_count]; + for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++) { - this->bufferLen[tmp_gazebo_id] = sizeof(CommunicationData) * this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); this->modelCounts = this->modelCounts + this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); - - this->receiveBufferLen = this->receiveBufferLen + this->bufferLen[tmp_gazebo_id]; - - this->displs[tmp_gazebo_id] = 0; - for (int k = 0; k < tmp_gazebo_id; k++) - this->displs[tmp_gazebo_id] = this->displs[tmp_gazebo_id] + this->bufferLen[k]; } + // this->receiveBufferLen = sizeof(CommunicationData) * this->modelCounts; } } //Added by zhangshuai based on zenglei 2019.03.19 ----End @@ -935,11 +928,23 @@ void World::Update() #endif // Added by zhangshuai 2019.04.03 for count time ----End - std::cout << "===== begin iterations:\t" << this->dataPtr->iterations << std::endl; int tmp_gazebo_count = this->distribution->GetGazeboCount(); char *sendBuffer; - char *receiveBuffer; + char *receiveBuffer; + + // for MPI_Allgatherv + + int *tmpbufferLen = new int[tmp_gazebo_count]; // array that its value is the number of data received from each process + int *tmpdispls = new int[tmp_gazebo_count]; // array that its value is the offset number of data received from each process + for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++) + { + tmpbufferLen[tmp_gazebo_id] = sizeof(CommunicationData) * this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); + + tmpdispls[tmp_gazebo_id] = 0; + for (int k = 0; k < tmp_gazebo_id; k++) + tmpdispls[tmp_gazebo_id] = tmpdispls[tmp_gazebo_id] + tmpbufferLen[k]; + } /// make the pose data ready unsigned int tmp_local_model_count = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelCount(); @@ -948,13 +953,12 @@ void World::Update() { 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(); - // std::cout << "======fuzhi:" << i << std::endl; - // std::cout << tmp_model_name << std::endl; tmp_model_name.clear(); } @@ -977,9 +981,7 @@ void World::Update() #endif // Added by zhangshuai 2019.04.03 for count time ----End - //gather poses to from all processes - std::cout << "======begin allgatherv:" << std::endl; - MPI_Allgatherv(sendBuffer, this->sendBufferLen, MPI_CHAR, receiveBuffer, bufferLen, displs, MPI_CHAR, MPI_COMM_WORLD); + MPI_Allgatherv(sendBuffer, this->sendBufferLen, MPI_CHAR, receiveBuffer, tmpbufferLen, tmpdispls, MPI_CHAR, MPI_COMM_WORLD); MPI_Barrier(MPI_COMM_WORLD); // Added by zhangshuai 2019.04.03 for count time ----Begin @@ -997,15 +999,16 @@ void World::Update() // Added by zhangshuai 2019.04.03 for count time ----End //receive and get information from other processes - std::cout << "======begin set pose:" << std::endl; for (int tmp_gazebo_id = 0; tmp_gazebo_id < tmp_gazebo_count; tmp_gazebo_id++) { if (this->gazeboLocalID != tmp_gazebo_id) { unsigned int tmp_model_count = this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); CommunicationData *tmpBufferData = new CommunicationData[tmp_model_count]; - tmpBufferData = (CommunicationData *)(receiveBuffer + displs[tmp_gazebo_id]); - for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(tmp_gazebo_id)->GetModelCount(); i++) + + memcpy(tmpBufferData, (receiveBuffer + tmpdispls[tmp_gazebo_id]), tmpbufferLen[tmp_gazebo_id]); + + 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); @@ -1015,10 +1018,10 @@ void World::Update() } } - delete[] sendBuffer; - delete[] receiveBuffer; delete[] sendBufferData; delete[] receiveBufferData; + delete[] tmpbufferLen; + delete[] tmpdispls; // Added by zhangshuai 2019.04.03 for count time ----Begin #ifdef USE_COUNT_TIME @@ -1027,7 +1030,6 @@ void World::Update() #endif // Added by zhangshuai 2019.04.03 for count time ----End - // MPI_Waitall(this->distribution->GetGazeboCount(), request, status); // MPI_Barrier(MPI_COMM_WORLD); } // MPI_Barrier(MPI_COMM_WORLD); @@ -1195,8 +1197,8 @@ void World::Fini() } //Added by zhangshuai 2019.06.10 ----Begin - delete[] this->bufferLen; - delete[] this->displs; + // delete[] this->bufferLen; + // delete[] this->displs; //Added by zhangshuai 2019.06.10 ----End } diff --git a/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh b/Gazebo_Distributed_MPI/mpi_run/MPI_Gazebo_launch.sh index 54f4566..5a427d7 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 50s +sleep 20s #登入各节点开始仿真 for((i=0;i