From 027189659834f685424c349c81ba3b0f8dd68e81 Mon Sep 17 00:00:00 2001 From: zhangshuai Date: Thu, 4 Apr 2019 16:01:05 +0800 Subject: [PATCH] Add judgement of flag to decide whether load plugins and sensors by distribution or not --- Gazebo_Distributed/gazebo/physics/Link.cc | 78 +++++++++++++++------- Gazebo_Distributed/gazebo/physics/World.cc | 39 +++++++---- Gazebo_Distributed/gazebo/physics/World.hh | 3 + 3 files changed, 85 insertions(+), 35 deletions(-) diff --git a/Gazebo_Distributed/gazebo/physics/Link.cc b/Gazebo_Distributed/gazebo/physics/Link.cc index 7724fb5..6ea8a1a 100644 --- a/Gazebo_Distributed/gazebo/physics/Link.cc +++ b/Gazebo_Distributed/gazebo/physics/Link.cc @@ -103,39 +103,71 @@ void Link::Load(sdf::ElementPtr _sdf) } // Modified by zhangshuai 2019.04.02 ----Begin - // Judge if the model to load is simulated in this gazebo - for (unsigned int i = 0; i < this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelCount(); i++) + if (this->GetWorld()->GetFlag() == 1) { - if (this->GetModel()->GetName() == this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelName(i)) + // Judge if the model to load is simulated in this gazebo + for (unsigned int i = 0; i < this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelCount(); i++) { - // Original part - if (this->sdf->HasElement("sensor")) + if (this->GetModel()->GetName() == this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelName(i)) { - sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor"); - while (sensorElem) + // Original part + if (this->sdf->HasElement("sensor")) { - /// \todo This if statement is a hack to prevent Links from creating - /// a force torque sensor. We should make this more generic. - if (sensorElem->Get("type") == "force_torque") + sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor"); + while (sensorElem) { - gzerr << "A link cannot load a [" << sensorElem->Get("type") << "] sensor.\n"; - } - else if (sensorElem->Get("type") != "__default__") - { - // This must match the implementation in Sensors::GetScopedName - std::string sensorName = this->GetScopedName(true) + "::" + - sensorElem->Get("name"); + /// \todo This if statement is a hack to prevent Links from creating + /// a force torque sensor. We should make this more generic. + if (sensorElem->Get("type") == "force_torque") + { + gzerr << "A link cannot load a [" << sensorElem->Get("type") << "] sensor.\n"; + } + else if (sensorElem->Get("type") != "__default__") + { + // This must match the implementation in Sensors::GetScopedName + std::string sensorName = this->GetScopedName(true) + "::" + + sensorElem->Get("name"); - // Tell the sensor library to create a sensor. - event::Events::createSensor(sensorElem, - this->GetWorld()->GetName(), this->GetScopedName(), this->GetId()); + // Tell the sensor library to create a sensor. + event::Events::createSensor(sensorElem, + this->GetWorld()->GetName(), this->GetScopedName(), this->GetId()); - this->sensors.push_back(sensorName); + this->sensors.push_back(sensorName); + } + sensorElem = sensorElem->GetNextElement("sensor"); } - sensorElem = sensorElem->GetNextElement("sensor"); } + // Original part + } + } + } + else + { + if (this->sdf->HasElement("sensor")) + { + sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor"); + while (sensorElem) + { + /// \todo This if statement is a hack to prevent Links from creating + /// a force torque sensor. We should make this more generic. + if (sensorElem->Get("type") == "force_torque") + { + gzerr << "A link cannot load a [" << sensorElem->Get("type") << "] sensor.\n"; + } + else if (sensorElem->Get("type") != "__default__") + { + // This must match the implementation in Sensors::GetScopedName + std::string sensorName = this->GetScopedName(true) + "::" + + sensorElem->Get("name"); + + // Tell the sensor library to create a sensor. + event::Events::createSensor(sensorElem, + this->GetWorld()->GetName(), this->GetScopedName(), this->GetId()); + + this->sensors.push_back(sensorName); + } + sensorElem = sensorElem->GetNextElement("sensor"); } - // Original part } } // Modified by zhangshuai 2019.04.02 ----End diff --git a/Gazebo_Distributed/gazebo/physics/World.cc b/Gazebo_Distributed/gazebo/physics/World.cc index 537c144..e09ee2e 100644 --- a/Gazebo_Distributed/gazebo/physics/World.cc +++ b/Gazebo_Distributed/gazebo/physics/World.cc @@ -277,9 +277,9 @@ void World::Load(sdf::ElementPtr _sdf) { sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution"); - gazeboLocalID = distributionElem->Get("gazebo_local_ID"); - port = distributionElem->Get("port"); - flag = distributionElem->Get("flag"); + this->gazeboLocalID = distributionElem->Get("gazebo_local_ID"); + this->port = distributionElem->Get("port"); + this->flag = distributionElem->Get("flag"); std::cout << "================= gazebo_local_ID: " << gazeboLocalID << "\t Port: " << port << "\t Flag: " << flag << " =================" << std::endl; @@ -454,7 +454,7 @@ void World::Run(unsigned int _iterations) this->dataPtr->stopIterations = _iterations; // Added by zhangshuai based on zenglei 2019.03.19 ----Begin - if (1 == flag) + if (1 == this->flag) { if (0 == this->gazeboLocalID) { @@ -550,7 +550,7 @@ void World::RunLoop() } // Added by zhangshuai based on zenglei 2019.03.19 ----Begin - if (1 == flag) + if (1 == this->flag) { if (0 == this->gazeboLocalID) { @@ -938,7 +938,7 @@ void World::Update() // Added by zhangshuai 2019.04.03 for count time ----End // Added by zhangshuai based on zenglei 2019.03.19 ----Begin - if (1 == flag) + if (1 == this->flag) { // if (this->dataPtr->iterations % 10 == 0) // Modified by zhangshuai 2019.04.02 ----Begin @@ -2373,16 +2373,24 @@ void World::ProcessFactoryMsgs() { model->Init(); // Modified by zhangshuai 2019.04.02 ----Begin - // Judge if the model to load is simulated in this gazebo - for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) + if (this->flag == 1) { - if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) + // Judge if the model to load is simulated in this gazebo + for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++) { - // Original part - model->LoadPlugins(); - // Original part + if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i)) + { + // Original part + model->LoadPlugins(); + // Original part + } } } + else + { + model->LoadPlugins(); + } + // Modified by zhangshuai 2019.04.02 ----End } } @@ -3303,4 +3311,11 @@ unsigned int World::GetGazeboLocalID() { return this->gazeboLocalID; } + +////////////////////////////////////////////////// +/// add for read the element of "flag" +unsigned int World::GetFlag() +{ + return this->flag; +} // Added by zhangshuai 2019.04.02 ----End \ No newline at end of file diff --git a/Gazebo_Distributed/gazebo/physics/World.hh b/Gazebo_Distributed/gazebo/physics/World.hh index cf4a6f7..c045fff 100644 --- a/Gazebo_Distributed/gazebo/physics/World.hh +++ b/Gazebo_Distributed/gazebo/physics/World.hh @@ -757,6 +757,9 @@ public: public: unsigned int GetGazeboLocalID(); + +public: + unsigned int GetFlag(); // Added by zhangshuai 2019.04.01 ----End }; /// \}