From ada6266e7946bb308f16642a88808043a1530964 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Sun, 28 Apr 2019 21:37:07 +0800 Subject: [PATCH] modify_2 --- ...rport_distribution_10hector_tcp_test.world | 80 ++++++ ...rport_distribution_30hector_tcp_test.world | 240 ++++++++++++++++++ .../gazebo/physics/World.cc | 32 ++- 3 files changed, 345 insertions(+), 7 deletions(-) diff --git a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_10hector_tcp_test.world b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_10hector_tcp_test.world index 8a90f3c..0f34ce7 100644 --- a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_10hector_tcp_test.world +++ b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_10hector_tcp_test.world @@ -50,6 +50,86 @@ --> + + + 0 0 0.186 0 0 0 + + model://quadrotor + + + + + + 0 5 0.186 0 0 0 + + model://quadrotor + + + + + + 0 10 0.186 0 0 0 + + model://quadrotor + + + + + + 0 15 0.186 0 0 0 + + model://quadrotor + + + + + + 0 20 0.186 0 0 0 + + model://quadrotor + + + + + + 0 25 0.186 0 0 0 + + model://quadrotor + + + + + + 0 30 0.186 0 0 0 + + model://quadrotor + + + + + + 0 35 0.186 0 0 0 + + model://quadrotor + + + + + + 0 40 0.186 0 0 0 + + model://quadrotor + + + + + + 0 45 0.186 0 0 0 + + model://quadrotor + + + diff --git a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_tcp_test.world b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_tcp_test.world index 12b6b77..75d2b3b 100644 --- a/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_tcp_test.world +++ b/Gazebo_Distributed_MPI/mpi_run/kunming_airport_distribution_30hector_tcp_test.world @@ -70,6 +70,246 @@ --> + + + 0 0 0.186 0 0 0 + + model://quadrotor + + + + + + 0 5 0.186 0 0 0 + + model://quadrotor + + + + + + 0 10 0.186 0 0 0 + + model://quadrotor + + + + + + 0 15 0.186 0 0 0 + + model://quadrotor + + + + + + 0 20 0.186 0 0 0 + + model://quadrotor + + + + + + 0 25 0.186 0 0 0 + + model://quadrotor + + + + + + 0 30 0.186 0 0 0 + + model://quadrotor + + + + + + 0 35 0.186 0 0 0 + + model://quadrotor + + + + + + 0 40 0.186 0 0 0 + + model://quadrotor + + + + + + 0 45 0.186 0 0 0 + + model://quadrotor + + + + + + 5 0 0.186 0 0 0 + + model://quadrotor + + + + + + 5 5 0.186 0 0 0 + + model://quadrotor + + + + + + 5 10 0.186 0 0 0 + + model://quadrotor + + + + + + 5 15 0.186 0 0 0 + + model://quadrotor + + + + + + 5 20 0.186 0 0 0 + + model://quadrotor + + + + + + 5 25 0.186 0 0 0 + + model://quadrotor + + + + + + 5 30 0.186 0 0 0 + + model://quadrotor + + + + + + 5 35 0.186 0 0 0 + + model://quadrotor + + + + + + 5 40 0.186 0 0 0 + + model://quadrotor + + + + + + 5 45 0.186 0 0 0 + + model://quadrotor + + + + + + 10 0 0.186 0 0 0 + + model://quadrotor + + + + + + 10 5 0.186 0 0 0 + + model://quadrotor + + + + + + 10 10 0.186 0 0 0 + + model://quadrotor + + + + + + 10 15 0.186 0 0 0 + + model://quadrotor + + + + + + 10 20 0.186 0 0 0 + + model://quadrotor + + + + + + 10 25 0.186 0 0 0 + + model://quadrotor + + + + + + 10 30 0.186 0 0 0 + + model://quadrotor + + + + + + 10 35 0.186 0 0 0 + + model://quadrotor + + + + + + 10 40 0.186 0 0 0 + + model://quadrotor + + + + + + 10 45 0.186 0 0 0 + + model://quadrotor + + + diff --git a/Gazebo_Distributed_TCP/gazebo/physics/World.cc b/Gazebo_Distributed_TCP/gazebo/physics/World.cc index b3c0743..da2ea15 100644 --- a/Gazebo_Distributed_TCP/gazebo/physics/World.cc +++ b/Gazebo_Distributed_TCP/gazebo/physics/World.cc @@ -958,11 +958,11 @@ void World::Update() { std::string tmp_model_name = this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i); unsigned int j = 0; - for (j = 0; j < this->GetModel(tmp_model_name)->GetLink("base_link")->GetName().length(); j++) - infor[i].link_name[j] = this->GetModel(tmp_model_name)->GetLink("base_link")->GetName()[j]; + for (j = 0; j < this->GetModel(tmp_model_name)->GetLink("canonical")->GetName().length(); j++) + infor[i].link_name[j] = this->GetModel(tmp_model_name)->GetLink("canonical")->GetName()[j]; infor[i].link_name[j] = '\0'; - infor[i].ID = this->GetModel(tmp_model_name)->GetLink("base_link")->GetId(); - infor[i].link_pose = this->GetModel(tmp_model_name)->GetLink("base_link")->GetWorldPose(); + infor[i].ID = this->GetModel(tmp_model_name)->GetLink("canonical")->GetId(); + infor[i].link_pose = this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose(); } CommunicationData temp[50]; @@ -1005,7 +1005,7 @@ void World::Update() for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(1)->GetModelCount(); i++) { std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i); - this->GetModel(tmp_model_name)->GetLink("base_link")->SetWorldPose(temp[i].link_pose); + this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(temp[i].link_pose); } // Added by zhangshuai 2019.04.03 for count time ----Begin @@ -1046,7 +1046,7 @@ void World::Update() for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(0)->GetModelCount(); i++) { std::string tmp_model_name = this->distribution->GetGazeboIDPtr(0)->GetModelName(i); - this->GetModel(tmp_model_name)->GetLink("base_link")->SetWorldPose(temp[i].link_pose); + this->GetModel(tmp_model_name)->GetLink("canonical")->SetWorldPose(temp[i].link_pose); } // Added by zhangshuai 2019.04.03 for count time ----Begin @@ -1862,7 +1862,25 @@ void World::LoadPlugins() { ModelPtr model = boost::static_pointer_cast( this->dataPtr->rootElement->GetChild(i)); - model->LoadPlugins(); + // Modified by zhangshuai 2019.04.02 ----Begin + if (this->flag == 1) + { + // Judge if the model to load is simulated in this gazebo + for (unsigned int j = 0; j < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); j++) + { + if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(j)) + { + // Original part + model->LoadPlugins(); + // Original part + } + } + } + else + { + model->LoadPlugins(); + } + // Modified by zhangshuai 2019.04.02 ----End } } }