From 85c423e5ea030cd07c7cef06779af78683abd9a7 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Sun, 28 Apr 2019 15:15:53 +0800 Subject: [PATCH] modiy --- .../gazebo/physics/World.cc | 286 +++++++++--------- 1 file changed, 146 insertions(+), 140 deletions(-) diff --git a/Gazebo_Distributed_MPI/gazebo/physics/World.cc b/Gazebo_Distributed_MPI/gazebo/physics/World.cc index 07a24d6..e639a2f 100644 --- a/Gazebo_Distributed_MPI/gazebo/physics/World.cc +++ b/Gazebo_Distributed_MPI/gazebo/physics/World.cc @@ -275,8 +275,14 @@ void World::Load(sdf::ElementPtr _sdf) if (this->dataPtr->sdf->HasElement("distribution")) { sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution"); - // int myid = 0; + int nproc; + char proc_name[MPI_MAX_PROCESSOR_NAME]; + int nameLength; MPI_Comm_rank(MPI_COMM_WORLD, &this->gazeboLocalID); + MPI_Comm_size(MPI_COMM_WORLD, &nproc); + MPI_Get_processor_name(proc_name, &nameLength); + printf("Gazebo, Iam host %s with rank %d of %d\n", proc_name, this->gazeboLocalID, nproc); + this->flag = distributionElem->Get("flag"); std::cout << "================= gazebo_local_ID: " << this->gazeboLocalID << "\t Flag: " << this->flag << " =================" << std::endl; @@ -898,150 +904,150 @@ void World::Update() #endif // Added by zhangshuai 2019.04.03 for count time ----End - // Added by zhangshuai 2019.04.23 for MPI communication----Begin - if (1 == this->flag) + // Added by zhangshuai 2019.04.23 for MPI communication----Begin + if (1 == this->flag) + { + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + // Added by zhangshuai 2019.04.03 for count time ----End + + MPI_Status *status = (MPI_Status *)malloc(sizeof(MPI_Status) * (this->distribution->GetGazeboCount())); + MPI_Request *request = (MPI_Request *)malloc(sizeof(MPI_Request) * (this->distribution->GetGazeboCount())); + + msgs::Pose_V modelPoseListSend; + + for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelCount(); i++) { - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End + msgs::Pose *modelPose; + modelPose = modelPoseListSend.add_pose(); - MPI_Status *status = (MPI_Status *)malloc(sizeof(MPI_Status) * (this->distribution->GetGazeboCount())); - MPI_Request *request = (MPI_Request *)malloc(sizeof(MPI_Request) * (this->distribution->GetGazeboCount())); + std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i); - msgs::Pose_V modelPoseListSend; + modelPose->set_name(tmp_model_name); + modelPose->set_id(this->GetModel(tmp_model_name)->GetLink("canonical")->GetId()); + modelPose->mutable_position()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.x); + modelPose->mutable_position()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.y); + modelPose->mutable_position()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.z); + modelPose->mutable_orientation()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.x); + modelPose->mutable_orientation()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.y); + modelPose->mutable_orientation()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.z); + modelPose->mutable_orientation()->set_w(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.w); - for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelCount(); i++) - { - msgs::Pose *modelPose; - modelPose = modelPoseListSend.add_pose(); - - std::string tmp_model_name = this->distribution->GetGazeboIDPtr(this->gazeboLocalID)->GetModelName(i); - - modelPose->set_name(tmp_model_name); - modelPose->set_id(this->GetModel(tmp_model_name)->GetLink("canonical")->GetId()); - modelPose->mutable_position()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.x); - modelPose->mutable_position()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.y); - modelPose->mutable_position()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().pos.z); - modelPose->mutable_orientation()->set_x(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.x); - modelPose->mutable_orientation()->set_y(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.y); - modelPose->mutable_orientation()->set_z(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.z); - modelPose->mutable_orientation()->set_w(this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose().rot.w); - - tmp_model_name.clear(); - } - - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - before_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End - - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End - - //send poses to another process - for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++) - { - if (this->gazeboLocalID != tmp_gazebo_id) - { - std::string send_messages = ""; - if (modelPoseListSend.pose_size() > 0) - { - modelPoseListSend.SerializeToString(&send_messages); - modelPoseListSend.clear_pose(); - } - - int bufferLen = send_messages.size(); - char *sendBuffer = new char[bufferLen + 1]; - for (int i = 0; i < bufferLen; i++) - { - sendBuffer[i] = send_messages[i]; - } - sendBuffer[bufferLen] = '\0'; - - send_messages.clear(); - - MPI_Isend(sendBuffer, bufferLen, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &request[tmp_gazebo_id]); - } - } - - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End - - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End - - //receive and get information from other processes - for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++) - { - if (this->gazeboLocalID != tmp_gazebo_id) - { - MPI_Probe(tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]); - int size; - MPI_Get_count(&status[tmp_gazebo_id], MPI_CHAR, &size); - char *receiveBuffer = (char *)malloc(sizeof(char) * (size)); - MPI_Recv(receiveBuffer, size, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]); - - if (size > 0) - { - std::string receive_messages; - for (int j = 0; j < size; j++) - { - receive_messages.push_back(receiveBuffer[j]); - } - - msgs::Pose_V modelPoseListReceive; - modelPoseListReceive.ParseFromString(receive_messages); - receive_messages.clear(); - - //insert models according to name,pose,vel from process 0 - for (int i = 0; i < modelPoseListReceive.pose_size(); i++) - { - math::Pose tmp_pose; - std::string tmp_model_name = modelPoseListReceive.pose(i).name(); - // std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i); - tmp_pose.pos.x = modelPoseListReceive.pose(i).position().x(); - tmp_pose.pos.y = modelPoseListReceive.pose(i).position().y(); - tmp_pose.pos.z = modelPoseListReceive.pose(i).position().z(); - tmp_pose.rot.x = modelPoseListReceive.pose(i).orientation().x(); - tmp_pose.rot.y = modelPoseListReceive.pose(i).orientation().y(); - tmp_pose.rot.z = modelPoseListReceive.pose(i).orientation().z(); - tmp_pose.rot.w = modelPoseListReceive.pose(i).orientation().w(); - - this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmp_pose); - } - } - } - } - - // Added by zhangshuai 2019.04.03 for count time ----Begin - #ifdef USE_COUNT_TIME - gettimeofday(&tv, NULL); - after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; - #endif - // Added by zhangshuai 2019.04.03 for count time ----End - - // MPI_Waitall(this->distribution->GetGazeboCount(), request, status); - // MPI_Barrier(MPI_COMM_WORLD); + tmp_model_name.clear(); } - MPI_Barrier(MPI_COMM_WORLD); - // Added by zhangshuai 2019.04.23 for MPI communication ----End + + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + before_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; +#endif + // Added by zhangshuai 2019.04.03 for count time ----End + + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + // Added by zhangshuai 2019.04.03 for count time ----End + + //send poses to another process + for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++) + { + if (this->gazeboLocalID != tmp_gazebo_id) + { + std::string send_messages = ""; + if (modelPoseListSend.pose_size() > 0) + { + modelPoseListSend.SerializeToString(&send_messages); + modelPoseListSend.clear_pose(); + } + + int bufferLen = send_messages.size(); + char *sendBuffer = new char[bufferLen + 1]; + for (int i = 0; i < bufferLen; i++) + { + sendBuffer[i] = send_messages[i]; + } + sendBuffer[bufferLen] = '\0'; + + send_messages.clear(); + + MPI_Isend(sendBuffer, bufferLen, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &request[tmp_gazebo_id]); + } + } + + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; +#endif + // Added by zhangshuai 2019.04.03 for count time ----End + +// Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6; +#endif + // Added by zhangshuai 2019.04.03 for count time ----End + + //receive and get information from other processes + for (int tmp_gazebo_id = 0; tmp_gazebo_id < this->distribution->GetGazeboCount(); tmp_gazebo_id++) + { + if (this->gazeboLocalID != tmp_gazebo_id) + { + MPI_Probe(tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]); + int size; + MPI_Get_count(&status[tmp_gazebo_id], MPI_CHAR, &size); + char *receiveBuffer = (char *)malloc(sizeof(char) * (size)); + MPI_Recv(receiveBuffer, size, MPI_CHAR, tmp_gazebo_id, 99, MPI_COMM_WORLD, &status[tmp_gazebo_id]); + + if (size > 0) + { + std::string receive_messages; + for (int j = 0; j < size; j++) + { + receive_messages.push_back(receiveBuffer[j]); + } + + msgs::Pose_V modelPoseListReceive; + modelPoseListReceive.ParseFromString(receive_messages); + receive_messages.clear(); + + //insert models according to name,pose,vel from process 0 + for (int i = 0; i < modelPoseListReceive.pose_size(); i++) + { + math::Pose tmp_pose; + std::string tmp_model_name = modelPoseListReceive.pose(i).name(); + // std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i); + tmp_pose.pos.x = modelPoseListReceive.pose(i).position().x(); + tmp_pose.pos.y = modelPoseListReceive.pose(i).position().y(); + tmp_pose.pos.z = modelPoseListReceive.pose(i).position().z(); + tmp_pose.rot.x = modelPoseListReceive.pose(i).orientation().x(); + tmp_pose.rot.y = modelPoseListReceive.pose(i).orientation().y(); + tmp_pose.rot.z = modelPoseListReceive.pose(i).orientation().z(); + tmp_pose.rot.w = modelPoseListReceive.pose(i).orientation().w(); + + this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(tmp_pose); + } + } + } + } + + // Added by zhangshuai 2019.04.03 for count time ----Begin +#ifdef USE_COUNT_TIME + gettimeofday(&tv, NULL); + after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time; +#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); + // Added by zhangshuai 2019.04.23 for MPI communication ----End // Only update state information if logging data. if (util::LogRecord::Instance()->Running())