pxmlw6n2f/Gazebo_Distributed_TCP/gazebo/physics/World.cc

/*
 * Copyright (C) 2012 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif

#include <time.h>

#include <tbb/parallel_for.h>
#include <tbb/blocked_range.h>

#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/recursive_mutex.hpp>

#include <sdf/sdf.hh>

#include <deque>
#include <list>
#include <set>
#include <string>
#include <vector>

#include <boost/algorithm/string/predicate.hpp>
#include <ignition/math/Rand.hh>
#include "gazebo/math/Rand.hh"

#include "gazebo/transport/Node.hh"
#include "gazebo/transport/TransportIface.hh"
#include "gazebo/transport/Publisher.hh"
#include "gazebo/transport/Subscriber.hh"

#include "gazebo/util/LogPlay.hh"

#include "gazebo/common/ModelDatabase.hh"
#include "gazebo/common/CommonIface.hh"
#include "gazebo/common/Events.hh"
#include "gazebo/common/Exception.hh"
#include "gazebo/common/Console.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/common/Time.hh"

#include "gazebo/math/Vector3.hh"

#include "gazebo/msgs/msgs.hh"

#include "gazebo/util/OpenAL.hh"
#include "gazebo/util/Diagnostics.hh"
#include "gazebo/util/LogRecord.hh"

#include "gazebo/physics/Road.hh"
#include "gazebo/physics/RayShape.hh"
#include "gazebo/physics/Link.hh"
#include "gazebo/physics/PhysicsEngine.hh"
#include "gazebo/physics/PhysicsFactory.hh"
#include "gazebo/physics/PresetManager.hh"
#include "gazebo/physics/UserCmdManager.hh"
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/Light.hh"
#include "gazebo/physics/Actor.hh"
#include "gazebo/physics/WorldPrivate.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/common/SphericalCoordinates.hh"

#include "gazebo/physics/Collision.hh"
#include "gazebo/physics/ContactManager.hh"
#include "gazebo/physics/Population.hh"

// Added by zhangshuai 2019.04.03 for count time ----Begin
#include "gazebo/countTime.hh"
// Added by zhangshuai 2019.04.03 for count time ----End

using namespace gazebo;
using namespace physics;

/// \brief Flag used to say if/when to clear all models.
/// This will be replaced with a class member variable in Gazebo 3.0
bool g_clearModels;

// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
double before_worldUpdateBeginTime = 0;
double worldUpdateBeginTime = 0;
double modelUpdateFuncTime = 0;
double updateCollisionTime = 0;
double loggingTime = 0;
double before_updatePhysicsTime = 0;
double updatePhysicsTime = 0;
double before_tcpTime = 0;
double tcpTime = 0;
double after_tcpTime = 0;
double wholeUpdataTime = 0;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End

class ModelUpdate_TBB
{
public:
  ModelUpdate_TBB(Model_V *_models) : models(_models) {}

public:
  void operator()(const tbb::blocked_range<size_t> &_r) const
  {
    for (size_t i = _r.begin(); i != _r.end(); i++)
    {
      (*models)[i]->Update();
    }
  }

private:
  Model_V *models;
};

//////////////////////////////////////////////////
World::World(const std::string &_name)
    : dataPtr(new WorldPrivate)
{
  g_clearModels = false;
  this->dataPtr->sdf.reset(new sdf::Element);
  sdf::initFile("world.sdf", this->dataPtr->sdf);

  // Keep this in the constructor for performance.
  // sdf::initFile causes disk access.
  this->dataPtr->factorySDF.reset(new sdf::SDF);
  sdf::initFile("root.sdf", this->dataPtr->factorySDF);

  this->dataPtr->logPlayStateSDF.reset(new sdf::Element);
  sdf::initFile("state.sdf", this->dataPtr->logPlayStateSDF);

  this->dataPtr->receiveMutex = new boost::recursive_mutex();
  this->dataPtr->loadModelMutex = new boost::mutex();

  this->dataPtr->initialized = false;
  this->dataPtr->loaded = false;
  this->dataPtr->stepInc = 0;
  this->dataPtr->pause = false;
  this->dataPtr->thread = nullptr;
  this->dataPtr->logThread = nullptr;
  this->dataPtr->stop = false;

  this->dataPtr->currentStateBuffer = 0;
  this->dataPtr->stateToggle = 0;

  this->dataPtr->pluginsLoaded = false;

  this->dataPtr->name = _name;

  this->dataPtr->needsReset = false;
  this->dataPtr->resetAll = true;
  this->dataPtr->resetTimeOnly = false;
  this->dataPtr->resetModelOnly = false;
  this->dataPtr->enablePhysicsEngine = true;
  this->dataPtr->setWorldPoseMutex = new boost::mutex();
  this->dataPtr->worldUpdateMutex = new boost::recursive_mutex();

  this->dataPtr->sleepOffset = common::Time(0);

  this->dataPtr->prevStatTime = common::Time::GetWallTime();
  this->dataPtr->prevProcessMsgsTime = common::Time::GetWallTime();

  this->dataPtr->connections.push_back(
      event::Events::ConnectStep(boost::bind(&World::OnStep, this)));
  this->dataPtr->connections.push_back(
      event::Events::ConnectPause(
          boost::bind(&World::SetPaused, this, _1)));
}

//////////////////////////////////////////////////
World::~World()
{
  this->Fini();

  delete this->dataPtr;
  this->dataPtr = nullptr;
}

//////////////////////////////////////////////////
void World::Load(sdf::ElementPtr _sdf)
{
  this->dataPtr->loaded = false;
  this->dataPtr->sdf = _sdf;

  if (this->dataPtr->sdf->Get<std::string>("name").empty())
    gzwarn << "create_world(world_name =["
           << this->dataPtr->name << "]) overwrites sdf world name\n!";
  else
    this->dataPtr->name = this->dataPtr->sdf->Get<std::string>("name");

#ifdef HAVE_OPENAL
  util::OpenAL::Instance()->Load(this->dataPtr->sdf->GetElement("audio"));
#endif

  this->dataPtr->sceneMsg.CopyFrom(
      msgs::SceneFromSDF(this->dataPtr->sdf->GetElement("scene")));
  this->dataPtr->sceneMsg.set_name(this->GetName());

  // The period at which messages are processed
  this->dataPtr->processMsgsPeriod = common::Time(0, 200000000);

  this->dataPtr->node = transport::NodePtr(new transport::Node());
  this->dataPtr->node->Init(this->GetName());

  // pose pub for server side, mainly used for updating and timestamping
  // Scene, which in turn will be used by rendering sensors.
  // TODO: replace local communication with shared memory for efficiency.
  this->dataPtr->poseLocalPub =
      this->dataPtr->node->Advertise<msgs::PosesStamped>("~/pose/local/info", 10);

  // pose pub for client with a cap on publishing rate to reduce traffic
  // overhead
  this->dataPtr->posePub = this->dataPtr->node->Advertise<msgs::PosesStamped>(
      "~/pose/info", 10, 60);

  this->dataPtr->guiPub = this->dataPtr->node->Advertise<msgs::GUI>("~/gui", 5);
  if (this->dataPtr->sdf->HasElement("gui"))
  {
    this->dataPtr->guiPub->Publish(
        msgs::GUIFromSDF(this->dataPtr->sdf->GetElement("gui")));
  }

  this->dataPtr->factorySub = this->dataPtr->node->Subscribe("~/factory",
                                                             &World::OnFactoryMsg, this);
  this->dataPtr->controlSub = this->dataPtr->node->Subscribe("~/world_control",
                                                             &World::OnControl, this);
  this->dataPtr->playbackControlSub = this->dataPtr->node->Subscribe(
      "~/playback_control", &World::OnPlaybackControl, this);

  this->dataPtr->requestSub = this->dataPtr->node->Subscribe("~/request",
                                                             &World::OnRequest, this, true);
  this->dataPtr->jointSub = this->dataPtr->node->Subscribe("~/joint",
                                                           &World::JointLog, this);

  this->dataPtr->lightSub = this->dataPtr->node->Subscribe("~/light",
                                                           &World::OnLightMsg, this);
  this->dataPtr->lightFactorySub =
      this->dataPtr->node->Subscribe("~/factory/light",
                                     &World::OnLightFactoryMsg, this);
  this->dataPtr->lightModifySub =
      this->dataPtr->node->Subscribe("~/light/modify",
                                     &World::OnLightModifyMsg, this);

  this->dataPtr->modelSub = this->dataPtr->node->Subscribe<msgs::Model>(
      "~/model/modify", &World::OnModelMsg, this);

  this->dataPtr->responsePub = this->dataPtr->node->Advertise<msgs::Response>(
      "~/response");
  this->dataPtr->statPub =
      this->dataPtr->node->Advertise<msgs::WorldStatistics>(
          "~/world_stats", 100, 5);
  this->dataPtr->modelPub = this->dataPtr->node->Advertise<msgs::Model>(
      "~/model/info");
  this->dataPtr->lightPub = this->dataPtr->node->Advertise<msgs::Light>(
      "~/light/modify");
  this->dataPtr->lightFactoryPub = this->dataPtr->node->Advertise<msgs::Light>(
      "~/factory/light");

  // Added by zhangshuai based on zenglei 2019.03.19 ----Begin
  if (this->dataPtr->sdf->HasElement("distribution"))
  {
    sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution");

    this->gazeboLocalID = distributionElem->Get<unsigned int>("gazebo_local_ID");
    this->port = distributionElem->Get<int>("port");
    this->flag = distributionElem->Get<int>("flag");

    std::cout << "================= gazebo_local_ID: " << gazeboLocalID << "\t Port: " << port << "\t Flag: " << flag << " =================" << std::endl;

    if (1 == this->flag)
    {
      DistributionPtr distribution_tmp(new physics::Distribution());

      this->distribution = distribution_tmp;
      this->distribution->Load(distributionElem);
    }

    std::cout << "================= gazebo_counts: " << this->distribution->GetGazeboCount() << " =================" << std::endl;
    std::cout << "================= gazebo_ID: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetGazeboID() << " =================" << std::endl;
    std::cout << "================= gazebo_ip: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetGazeboIp() << " =================" << std::endl;
    std::cout << "================= model_counts: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount() << " =================" << std::endl;
    // std::cout << "================= the second model name: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(1) << " =================" << std::endl;

    // ip = eventElem->Get<std::string>("ip");
    // simula_entity_num = eventElem->Get<int>("simula_entity_num");
    // shadow_entity_num = eventElem->Get<int>("shadow_entity_num");
  }
  //Added by zhangshuai based on zenglei 2019.03.19 ----End

  // This should come before loading of entities
  sdf::ElementPtr physicsElem = this->dataPtr->sdf->GetElement("physics");

  std::string type = physicsElem->Get<std::string>("type");
  this->dataPtr->physicsEngine = PhysicsFactory::NewPhysicsEngine(type,
                                                                  shared_from_this());

  if (this->dataPtr->physicsEngine == nullptr)
    gzthrow("Unable to create physics engine\n");

  this->dataPtr->physicsEngine->Load(physicsElem);

  // This should also come before loading of entities
  {
    sdf::ElementPtr spherical = this->dataPtr->sdf->GetElement(
        "spherical_coordinates");
    common::SphericalCoordinates::SurfaceType surfaceType =
        common::SphericalCoordinates::Convert(
            spherical->Get<std::string>("surface_model"));
    ignition::math::Angle latitude, longitude, heading;
    double elevation = spherical->Get<double>("elevation");
    latitude.Degree(spherical->Get<double>("latitude_deg"));
    longitude.Degree(spherical->Get<double>("longitude_deg"));
    heading.Degree(spherical->Get<double>("heading_deg"));

    this->dataPtr->sphericalCoordinates.reset(new common::SphericalCoordinates(
        surfaceType, latitude, longitude, elevation, heading));
  }

  if (this->dataPtr->sphericalCoordinates == nullptr)
    gzthrow("Unable to create spherical coordinates data structure\n");

  this->dataPtr->rootElement.reset(new Base(BasePtr()));
  this->dataPtr->rootElement->SetName(this->GetName());
  this->dataPtr->rootElement->SetWorld(shared_from_this());

  // A special order is necessary when loading a world that contains state
  // information. The joints must be created last, otherwise they get
  // initialized improperly.
  {
    // Create all the entities
    this->LoadEntities(this->dataPtr->sdf, this->dataPtr->rootElement);

    for (unsigned int i = 0; i < this->GetModelCount(); ++i)
      this->GetModel(i)->LoadJoints();
  }

  // TODO: Performance test to see if TBB model updating is necessary
  // Choose threaded or unthreaded model updating depending on the number of
  // models in the scene
  // if (this->GetModelCount() < 20)
  this->dataPtr->modelUpdateFunc = &World::ModelUpdateSingleLoop;
  // else
  // this->dataPtr->modelUpdateFunc = &World::ModelUpdateTBB;

  event::Events::worldCreated(this->GetName());

  this->dataPtr->userCmdManager = UserCmdManagerPtr(
      new UserCmdManager(shared_from_this()));

  this->dataPtr->loaded = true;
}

//////////////////////////////////////////////////
void World::Save(const std::string &_filename)
{
  this->UpdateStateSDF();
  std::string data;
  data = "<?xml version ='1.0'?>\n";
  data += "<sdf version='" +
          boost::lexical_cast<std::string>(SDF_VERSION) + "'>\n";
  data += this->dataPtr->sdf->ToString("");
  data += "</sdf>\n";

  std::ofstream out(_filename.c_str(), std::ios::out);
  if (!out)
    gzerr << "Unable to open file[" << _filename << "]\n";
  else
    out << data;

  out.close();
}

//////////////////////////////////////////////////
void World::Init()
{
  // Initialize all the entities (i.e. Model)
  for (unsigned int i = 0; i < this->dataPtr->rootElement->GetChildCount(); i++)
    this->dataPtr->rootElement->GetChild(i)->Init();

  // Initialize the physics engine
  this->dataPtr->physicsEngine->Init();

  this->dataPtr->presetManager = PresetManagerPtr(
      new PresetManager(this->dataPtr->physicsEngine, this->dataPtr->sdf));

  this->dataPtr->testRay = boost::dynamic_pointer_cast<RayShape>(
      this->GetPhysicsEngine()->CreateShape("ray", CollisionPtr()));

  this->dataPtr->prevStates[0].SetWorld(shared_from_this());
  this->dataPtr->prevStates[1].SetWorld(shared_from_this());

  this->dataPtr->prevStates[0].SetName(this->GetName());
  this->dataPtr->prevStates[1].SetName(this->GetName());

  this->dataPtr->updateInfo.worldName = this->GetName();

  this->dataPtr->iterations = 0;
  this->dataPtr->logPrevIteration = 0;

  util::DiagnosticManager::Instance()->Init(this->GetName());

  util::LogRecord::Instance()->Add(this->GetName(), "state.log",
                                   boost::bind(&World::OnLog, this, _1));

  // Check if we have to insert an object population.
  if (this->dataPtr->sdf->HasElement("population"))
  {
    Population population(this->dataPtr->sdf, shared_from_this());
    population.PopulateAll();
  }

  // Set the state of the entities
  if (this->dataPtr->sdf->HasElement("state"))
  {
    sdf::ElementPtr childElem = this->dataPtr->sdf->GetElement("state");

    while (childElem)
    {
      WorldState myState;
      myState.Load(childElem);
      this->SetState(myState);

      childElem = childElem->GetNextElement("state");

      // TODO: We currently load just the first state data. Need to
      // implement a better mechanism for handling multiple states
      break;
    }
  }

  this->dataPtr->initialized = true;

  // Mark the world initialization
  gzlog << "Init world[" << this->GetName() << "]" << std::endl;
}

//////////////////////////////////////////////////
void World::Run(unsigned int _iterations)
{
  this->dataPtr->stop = false;
  this->dataPtr->stopIterations = _iterations;

  // Added by zhangshuai based on zenglei 2019.03.19 ----Begin
  if (1 == this->flag)
  {
    if (0 == this->gazeboLocalID)
    {
      gazeboZero.SetPort(port);
      gazeboZero.StartServer();
    }
    else if (1 == this->gazeboLocalID)
    {
      gazeboOne.SetIp(this->distribution->GetGazeboIDPtr(0)->GetGazeboIp());
      gazeboOne.SetPort(port);
      gazeboOne.ConnectServer();
    }
  }
  // Added by zhangshuai based on zenglei 2019.03.19 ----End

  this->dataPtr->thread = new boost::thread(boost::bind(&World::RunLoop, this));
}

//////////////////////////////////////////////////
void World::RunBlocking(unsigned int _iterations)
{
  this->dataPtr->stop = false;
  this->dataPtr->stopIterations = _iterations;
  this->RunLoop();
}

//////////////////////////////////////////////////
void World::RemoveModel(ModelPtr _model)
{
  if (_model)
    this->RemoveModel(_model->GetName());
}

//////////////////////////////////////////////////
bool World::GetRunning() const
{
  return !this->dataPtr->stop;
}

//////////////////////////////////////////////////
void World::Stop()
{
  this->dataPtr->stop = true;

  if (this->dataPtr->thread)
  {
    this->dataPtr->thread->join();
    delete this->dataPtr->thread;
    this->dataPtr->thread = nullptr;
  }
}

//////////////////////////////////////////////////
void World::RunLoop()
{
  this->dataPtr->physicsEngine->InitForThread();

  this->dataPtr->startTime = common::Time::GetWallTime();

  // This fixes a minor issue when the world is paused before it's started
  if (this->IsPaused())
    this->dataPtr->pauseStartTime = this->dataPtr->startTime;

  this->dataPtr->prevStepWallTime = common::Time::GetWallTime();

  // Get the first state
  this->dataPtr->prevStates[0] = WorldState(shared_from_this());
  this->dataPtr->prevStates[1] = WorldState(shared_from_this());

  this->dataPtr->stateToggle = 0;

  this->dataPtr->logThread =
      new boost::thread(boost::bind(&World::LogWorker, this));

  if (!util::LogPlay::Instance()->IsOpen())
  {
    for (this->dataPtr->iterations = 0; !this->dataPtr->stop &&
                                        (!this->dataPtr->stopIterations ||
                                         (this->dataPtr->iterations < this->dataPtr->stopIterations));)
    {
      this->Step();
    }
  }
  else
  {
    this->dataPtr->enablePhysicsEngine = false;
    for (this->dataPtr->iterations = 0; !this->dataPtr->stop &&
                                        (!this->dataPtr->stopIterations ||
                                         (this->dataPtr->iterations < this->dataPtr->stopIterations));)
    {
      this->LogStep();
    }
  }

  // Added by zhangshuai based on zenglei 2019.03.19 ----Begin
  if (1 == this->flag)
  {
    if (0 == this->gazeboLocalID)
    {
      gazeboZero.Close();
    }
    else if (1 == this->gazeboLocalID)
    {
      gazeboOne.Close();
    }
  }
  // Added by zhangshuai based on zenglei 2019.03.19 ----End

  this->dataPtr->stop = true;

  if (this->dataPtr->logThread)
  {
    this->dataPtr->logCondition.notify_all();
    {
      boost::mutex::scoped_lock lock(this->dataPtr->logMutex);
      this->dataPtr->logCondition.notify_all();
    }
    this->dataPtr->logThread->join();
    delete this->dataPtr->logThread;
    this->dataPtr->logThread = nullptr;
  }
}

//////////////////////////////////////////////////
void World::LogStep()
{
  {
    boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);

    if (!this->IsPaused() || this->dataPtr->stepInc != 0)
    {
      if (!this->IsPaused() && this->dataPtr->stepInc == 0)
        this->dataPtr->stepInc = 1;

      std::string data;
      if (!util::LogPlay::Instance()->Step(this->dataPtr->stepInc, data))
      {
        // There are no more chunks, time to exit.
        this->SetPaused(true);
        this->dataPtr->stepInc = 0;
      }
      else
      {
        this->dataPtr->stepInc = 1;

        this->dataPtr->logPlayStateSDF->ClearElements();
        sdf::readString(data, this->dataPtr->logPlayStateSDF);

        this->dataPtr->logPlayState.Load(this->dataPtr->logPlayStateSDF);

        // If the log file does not contain iterations we have to manually
        // increase the iteration counter in logPlayState.
        if (!util::LogPlay::Instance()->HasIterations())
        {
          this->dataPtr->logPlayState.SetIterations(
              this->dataPtr->iterations + 1);
        }

        this->SetState(this->dataPtr->logPlayState);
        this->Update();
      }

      if (this->dataPtr->stepInc > 0)
        this->dataPtr->stepInc--;
    }
  }

  this->PublishWorldStats();

  this->ProcessMessages();
}

//////////////////////////////////////////////////
void World::_SetSensorsInitialized(const bool _init)
{
  this->dataPtr->sensorsInitialized = _init;
}

//////////////////////////////////////////////////
bool World::SensorsInitialized() const
{
  return this->dataPtr->sensorsInitialized;
}

//////////////////////////////////////////////////
void World::Step()
{
  DIAG_TIMER_START("World::Step");

  /// need this because ODE does not call dxReallocateWorldProcessContext()
  /// until dWorld.*Step
  /// Plugins that manipulate joints (and probably other properties) require
  /// one iteration of the physics engine. Do not remove this.
  if (!this->dataPtr->pluginsLoaded && this->SensorsInitialized())
  {
    this->LoadPlugins();
    this->dataPtr->pluginsLoaded = true;
  }

  DIAG_TIMER_LAP("World::Step", "loadPlugins");

  // Send statistics about the world simulation
  this->PublishWorldStats();

  DIAG_TIMER_LAP("World::Step", "publishWorldStats");

  double updatePeriod = this->dataPtr->physicsEngine->GetUpdatePeriod();
  // sleep here to get the correct update rate
  common::Time tmpTime = common::Time::GetWallTime();
  common::Time sleepTime = this->dataPtr->prevStepWallTime +
                           common::Time(updatePeriod) - tmpTime - this->dataPtr->sleepOffset;

  common::Time actualSleep = 0;
  if (sleepTime > 0)
  {
    common::Time::Sleep(sleepTime);
    actualSleep = common::Time::GetWallTime() - tmpTime;
  }
  else
    sleepTime = 0;

  // exponentially avg out
  this->dataPtr->sleepOffset = (actualSleep - sleepTime) * 0.01 +
                               this->dataPtr->sleepOffset * 0.99;

  DIAG_TIMER_LAP("World::Step", "sleepOffset");

  // throttling update rate, with sleepOffset as tolerance
  // the tolerance is needed as the sleep time is not exact
  if (common::Time::GetWallTime() - this->dataPtr->prevStepWallTime +
          this->dataPtr->sleepOffset >=
      common::Time(updatePeriod))
  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);

    DIAG_TIMER_LAP("World::Step", "worldUpdateMutex");

    this->dataPtr->prevStepWallTime = common::Time::GetWallTime();

    double stepTime = this->dataPtr->physicsEngine->GetMaxStepSize();

    if (!this->IsPaused() || this->dataPtr->stepInc > 0 || this->dataPtr->needsReset)
    {
      // query timestep to allow dynamic time step size updates
      this->dataPtr->simTime += stepTime;
      this->dataPtr->iterations++;
      this->Update();

      DIAG_TIMER_LAP("World::Step", "update");

      if (this->IsPaused() && this->dataPtr->stepInc > 0)
        this->dataPtr->stepInc--;
    }
    else
    {
      // Flush the log record buffer, if there is data in it.
      if (util::LogRecord::Instance()->BufferSize() > 0)
        util::LogRecord::Instance()->Notify();
      this->dataPtr->pauseTime += stepTime;
    }
  }

  this->ProcessMessages();

  DIAG_TIMER_STOP("World::Step");

  if (g_clearModels)
    this->ClearModels();
}

//////////////////////////////////////////////////
void World::Step(unsigned int _steps)
{
  if (!this->IsPaused())
  {
    gzwarn << "Calling World::Step(steps) while world is not paused\n";
    this->SetPaused(true);
  }

  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
    this->dataPtr->stepInc = _steps;
  }

  // block on completion
  bool wait = true;
  while (wait)
  {
    common::Time::NSleep(1);
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
    if (this->dataPtr->stepInc == 0 || this->dataPtr->stop)
      wait = false;
  }
}

//////////////////////////////////////////////////
void World::Update()
{

// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  struct timeval tv;
  double cur_time;
  double start_time;
  gettimeofday(&tv, NULL);
  cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
  start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
  // Added by zhangshuai 2019.04.03 for count time ----End

  DIAG_TIMER_START("World::Update");

  if (this->dataPtr->needsReset)
  {
    if (this->dataPtr->resetAll)
      this->Reset();
    else if (this->dataPtr->resetTimeOnly)
      this->ResetTime();
    else if (this->dataPtr->resetModelOnly)
      this->ResetEntities(Base::MODEL);
    this->dataPtr->needsReset = false;
    return;
  }
  DIAG_TIMER_LAP("World::Update", "needsReset");

  this->dataPtr->updateInfo.simTime = this->GetSimTime();
  this->dataPtr->updateInfo.realTime = this->GetRealTime();

// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  before_worldUpdateBeginTime += (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

  event::Events::worldUpdateBegin(this->dataPtr->updateInfo);

// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  worldUpdateBeginTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
  // Added by zhangshuai 2019.04.03 for count time ----End

  DIAG_TIMER_LAP("World::Update", "Events::worldUpdateBegin");

  // 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

  // Update all the models
  (*this.*dataPtr->modelUpdateFunc)();

  // Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  modelUpdateFuncTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
  // Added by zhangshuai 2019.04.03 for count time ----End

  DIAG_TIMER_LAP("World::Update", "Model::Update");

  // 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

  // This must be called before PhysicsEngine::UpdatePhysics.
  this->dataPtr->physicsEngine->UpdateCollision();

  // Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  updateCollisionTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
  // Added by zhangshuai 2019.04.03 for count time ----End

  DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdateCollision");

  // 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

  // Wait for logging to finish, if it's running.
  if (util::LogRecord::Instance()->Running())
  {
    boost::mutex::scoped_lock lock(this->dataPtr->logMutex);

    // It's possible the logWorker thread never processed the previous
    // state. This checks to make sure that we don't continute until the log
    // worker catchs up.
    if (this->dataPtr->iterations - this->dataPtr->logPrevIteration > 1)
    {
      this->dataPtr->logCondition.notify_one();
      this->dataPtr->logContinueCondition.wait(lock);
    }
  }

  // Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  loggingTime += (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

  // Give clients a possibility to react to collisions before the physics
  // gets updated.
  this->dataPtr->updateInfo.realTime = this->GetRealTime();
  event::Events::beforePhysicsUpdate(this->dataPtr->updateInfo);

  // Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  before_updatePhysicsTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
  // Added by zhangshuai 2019.04.03 for count time ----End

  DIAG_TIMER_LAP("World::Update", "Events::beforePhysicsUpdate");

  // 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

  // Update the physics engine
  if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
  {
    // This must be called directly after PhysicsEngine::UpdateCollision.
    this->dataPtr->physicsEngine->UpdatePhysics();

    DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdatePhysics");

    // do this after physics update as
    //   ode --> MoveCallback sets the dirtyPoses
    //           and we need to propagate it into Entity::worldPose
    {
      // block any other pose updates (e.g. Joint::SetPosition)
      boost::recursive_mutex::scoped_lock lock(
          *this->dataPtr->physicsEngine->GetPhysicsUpdateMutex());

      for (auto &dirtyEntity : this->dataPtr->dirtyPoses)
      {
        dirtyEntity->SetWorldPose(dirtyEntity->GetDirtyPose(), false);
      }

      this->dataPtr->dirtyPoses.clear();
    }

    DIAG_TIMER_LAP("World::Update", "SetWorldPose(dirtyPoses)");
  }

  // Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  updatePhysicsTime += (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 based on zenglei 2019.03.19 ----Begin
  if (1 == this->flag)
  {
    // if (this->dataPtr->iterations % 10 == 0)
    // Modified by zhangshuai 2019.04.02 ----Begin
    {

      // 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

      for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++)
      {
        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("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("canonical")->GetId();
        infor[i].link_pose = this->GetModel(tmp_model_name)->GetLink("canonical")->GetWorldPose();
      }

      CommunicationData temp[50];

      // 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

      if (0 == this->gazeboLocalID)
      {
        // 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

        if (!gazeboZero.sendInfor(infor, this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount()))
          std::cout << "Send Infor fail!" << std::endl;
        if (!gazeboZero.recvInfor(temp, this->distribution->GetGazeboIDPtr(1)->GetModelCount()))
          std::cout << "Rcceive Infor fail!" << std::endl;

          // 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

        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("canonical")->SetWorldPose(temp[i].link_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
      }
      else if (1 == this->gazeboLocalID)
      {
        // 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

        if (!gazeboOne.recvInfor(temp, this->distribution->GetGazeboIDPtr(0)->GetModelCount()))
          std::cout << "Rcceive Infor fail!" << std::endl;
        if (!gazeboOne.sendInfor(infor, this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount()))
          std::cout << "Send Infor fail!" << std::endl;

          // 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

        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("canonical")->SetWorldPose(temp[i].link_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
      }
    }
    // Modified by zhangshuai 2019.04.02 ----End
  }
  // Added by zhangshuai based on zenglei 2019.03.19 ----End

  // Only update state information if logging data.
  if (util::LogRecord::Instance()->Running())
    this->dataPtr->logCondition.notify_one();
  DIAG_TIMER_LAP("World::Update", "LogRecordNotify");

  // Output the contact information
  this->dataPtr->physicsEngine->GetContactManager()->PublishContacts();

  DIAG_TIMER_LAP("World::Update", "ContactManager::PublishContacts");

  event::Events::worldUpdateEnd();

  DIAG_TIMER_STOP("World::Update");

#ifdef USE_COUNT_TIME
  gettimeofday(&tv, NULL);
  wholeUpdataTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time;
#endif

  if (this->dataPtr->iterations == 100000)
  {
    std::cout << "===== average before_worldUpdateBeginTime:\t" << before_worldUpdateBeginTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average worldUpdateBeginTime:\t" << worldUpdateBeginTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average modelUpdateFuncTime:\t" << modelUpdateFuncTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average updateCollisionTime:\t" << updateCollisionTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average loggingTime:\t" << loggingTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average before_updatePhysicsTime:\t" << before_updatePhysicsTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average updatePhysicsTime:\t" << updatePhysicsTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average before_tcpTime:\t" << before_tcpTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average tcpTime:\t" << tcpTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average after_tcpTime:\t" << after_tcpTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      double wholeTcpTime = before_tcpTime + tcpTime + after_tcpTime;
      std::cout << "===== average wholeTcpTime:\t" << wholeTcpTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_tcpTime + tcpTime + after_tcpTime;
      std::cout << "===== average wholeAddTime:\t" << wholeAddTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
      std::cout << "===== average wholeUpdateTime:\t" << wholeUpdataTime * 1000.0 / this->dataPtr->iterations << "\tms =====" << std::endl;
  }
}

//////////////////////////////////////////////////
void World::Fini()
{
  this->dataPtr->stop = true;

#ifdef HAVE_OPENAL
  util::OpenAL::Instance()->Fini();
#endif

  // Clean transport
  {
    this->dataPtr->deleteEntity.clear();
    this->dataPtr->requestMsgs.clear();
    this->dataPtr->factoryMsgs.clear();
    this->dataPtr->modelMsgs.clear();
    this->dataPtr->lightFactoryMsgs.clear();
    this->dataPtr->lightModifyMsgs.clear();
    this->dataPtr->playbackControlMsgs.clear();

    this->dataPtr->poseLocalPub.reset();
    this->dataPtr->posePub.reset();
    this->dataPtr->guiPub.reset();
    this->dataPtr->responsePub.reset();
    this->dataPtr->statPub.reset();
    this->dataPtr->modelPub.reset();
    this->dataPtr->lightPub.reset();
    this->dataPtr->lightFactoryPub.reset();

    this->dataPtr->factorySub.reset();
    this->dataPtr->controlSub.reset();
    this->dataPtr->playbackControlSub.reset();
    this->dataPtr->requestSub.reset();
    this->dataPtr->jointSub.reset();
    this->dataPtr->lightSub.reset();
    this->dataPtr->lightFactorySub.reset();
    this->dataPtr->lightModifySub.reset();
    this->dataPtr->modelSub.reset();

    this->dataPtr->node.reset();
  }

  this->dataPtr->connections.clear();

  this->dataPtr->sdf.reset();

  this->dataPtr->testRay.reset();
  this->dataPtr->plugins.clear();

  this->dataPtr->publishModelPoses.clear();
  this->dataPtr->publishModelScales.clear();
  this->dataPtr->publishLightPoses.clear();

  // Clean entities
  for (auto &model : this->dataPtr->models)
  {
    if (model)
      model->Fini();
  }
  this->dataPtr->models.clear();

  for (auto &light : this->dataPtr->lights)
  {
    if (light)
      light->Fini();
  }
  this->dataPtr->lights.clear();

  if (this->dataPtr->rootElement)
  {
    this->dataPtr->rootElement->Fini();
    this->dataPtr->rootElement.reset();
  }
  this->dataPtr->prevStates[0].SetWorld(WorldPtr());
  this->dataPtr->prevStates[1].SetWorld(WorldPtr());
  this->dataPtr->prevUnfilteredState.SetWorld(WorldPtr());
  this->dataPtr->logPlayState.SetWorld(WorldPtr());
  this->dataPtr->states[0].clear();
  this->dataPtr->states[1].clear();

  this->dataPtr->presetManager.reset();
  this->dataPtr->userCmdManager.reset();
  this->dataPtr->physicsEngine.reset();

  // Clean mutexes
  if (this->dataPtr->receiveMutex)
  {
    delete this->dataPtr->receiveMutex;
    this->dataPtr->receiveMutex = nullptr;
  }

  if (this->dataPtr->loadModelMutex)
  {
    delete this->dataPtr->loadModelMutex;
    this->dataPtr->loadModelMutex = nullptr;
  }

  if (this->dataPtr->setWorldPoseMutex)
  {
    delete this->dataPtr->setWorldPoseMutex;
    this->dataPtr->setWorldPoseMutex = nullptr;
  }

  if (this->dataPtr->worldUpdateMutex)
  {
    delete this->dataPtr->worldUpdateMutex;
    this->dataPtr->worldUpdateMutex = nullptr;
  }

  // End world run thread
  if (this->dataPtr->thread)
  {
    this->dataPtr->thread->join();
    delete this->dataPtr->thread;
    this->dataPtr->thread = nullptr;
  }
}

//////////////////////////////////////////////////
void World::Clear()
{
  g_clearModels = true;
  /// \todo Clear lights too?
}

//////////////////////////////////////////////////
void World::ClearModels()
{
  g_clearModels = false;
  bool pauseState = this->IsPaused();
  this->SetPaused(true);

  while (!this->dataPtr->models.empty())
  {
    this->RemoveModel(this->dataPtr->models[0]);
  }
  this->dataPtr->models.clear();

  this->SetPaused(pauseState);
}

//////////////////////////////////////////////////
std::string World::GetName() const
{
  return this->dataPtr->name;
}

//////////////////////////////////////////////////
PhysicsEnginePtr World::GetPhysicsEngine() const
{
  return this->dataPtr->physicsEngine;
}

//////////////////////////////////////////////////
PresetManagerPtr World::GetPresetManager() const
{
  return this->dataPtr->presetManager;
}

//////////////////////////////////////////////////
common::SphericalCoordinatesPtr World::GetSphericalCoordinates() const
{
  return this->dataPtr->sphericalCoordinates;
}

//////////////////////////////////////////////////
ignition::math::Vector3d World::Gravity() const
{
  return this->dataPtr->sdf->Get<ignition::math::Vector3d>("gravity");
}

//////////////////////////////////////////////////
void World::SetGravity(const ignition::math::Vector3d &_gravity)
{
  // This function calls `PhysicsEngine::SetGravity`,
  // which in turn should call `World::SetGravitySDF`.
  this->dataPtr->physicsEngine->SetGravity(_gravity);
}

//////////////////////////////////////////////////
void World::SetGravitySDF(const ignition::math::Vector3d &_gravity)
{
  this->dataPtr->sdf->GetElement("gravity")->Set(_gravity);
}

//////////////////////////////////////////////////
ignition::math::Vector3d World::MagneticField() const
{
  return this->dataPtr->sdf->Get<ignition::math::Vector3d>("magnetic_field");
}

//////////////////////////////////////////////////
void World::SetMagneticField(const ignition::math::Vector3d &_mag)
{
  this->dataPtr->sdf->GetElement("magnetic_field")->Set(_mag);
}

//////////////////////////////////////////////////
BasePtr World::GetByName(const std::string &_name)
{
  if (this->dataPtr->rootElement)
    return this->dataPtr->rootElement->GetByName(_name);
  else
    return BasePtr();
}

/////////////////////////////////////////////////
ModelPtr World::GetModelById(unsigned int _id)
{
  return boost::dynamic_pointer_cast<Model>(
      this->dataPtr->rootElement->GetById(_id));
}

//////////////////////////////////////////////////
ModelPtr World::GetModel(const std::string &_name)
{
  boost::mutex::scoped_lock lock(*this->dataPtr->loadModelMutex);
  return boost::dynamic_pointer_cast<Model>(this->GetByName(_name));
}

//////////////////////////////////////////////////
LightPtr World::Light(const std::string &_name)
{
  std::lock_guard<std::mutex> lock(this->dataPtr->loadLightMutex);
  return boost::dynamic_pointer_cast<physics::Light>(this->GetByName(_name));
}

//////////////////////////////////////////////////
EntityPtr World::GetEntity(const std::string &_name)
{
  return boost::dynamic_pointer_cast<Entity>(this->GetByName(_name));
}

//////////////////////////////////////////////////
ModelPtr World::LoadModel(sdf::ElementPtr _sdf, BasePtr _parent)
{
  boost::mutex::scoped_lock lock(*this->dataPtr->loadModelMutex);
  ModelPtr model;

  if (_sdf->GetName() == "model")
  {
    std::string modelName = _sdf->Get<std::string>("name");
    for (auto const m : this->dataPtr->models)
    {
      if (m->GetName() == modelName)
      {
        gzwarn << "Model with name [" << modelName << "] already exists. "
               << "Not inserting model. This warning can be ignored in certain "
               << "situations such as rewind during log playback.\n";
        return model;
      }
    }

    model = this->dataPtr->physicsEngine->CreateModel(_parent);
    model->SetWorld(shared_from_this());
    model->Load(_sdf);

    event::Events::addEntity(model->GetScopedName());

    msgs::Model msg;
    model->FillMsg(msg);
    this->dataPtr->modelPub->Publish(msg);

    this->EnableAllModels();
  }
  else
  {
    gzerr << "SDF is missing the <model> tag:\n";
  }

  this->PublishModelPose(model);
  this->dataPtr->models.push_back(model);
  return model;
}

//////////////////////////////////////////////////
LightPtr World::LoadLight(const sdf::ElementPtr &_sdf, const BasePtr &_parent)
{
  std::lock_guard<std::mutex> lock(this->dataPtr->loadLightMutex);

  if (_sdf->GetName() != "light")
  {
    gzerr << "SDF is missing the <light> tag" << std::endl;
    return nullptr;
  }

  // Add to scene message
  msgs::Light *msg = this->dataPtr->sceneMsg.add_light();
  msg->CopyFrom(msgs::LightFromSDF(_sdf));

  // Create new light object
  LightPtr light(new physics::Light(_parent));
  light->ProcessMsg(*msg);
  light->SetWorld(shared_from_this());
  light->Load(_sdf);
  this->dataPtr->lights.push_back(light);

  // msg should contain scoped name (consistent with other entities)
  msg->set_name(light->GetScopedName());

  // publish after adding light to the lights vector
  // we also process light factory msg in World so this avoids creating a
  // duplicate light
  // Note: models uses /model/info topic. We can consider adding a
  // /light/info topic for this, see issue #2288
  this->dataPtr->lightFactoryPub->Publish(*msg);

  return light;
}

//////////////////////////////////////////////////
ActorPtr World::LoadActor(sdf::ElementPtr _sdf, BasePtr _parent)
{
  ActorPtr actor(new Actor(_parent));
  actor->SetWorld(shared_from_this());
  actor->Load(_sdf);

  event::Events::addEntity(actor->GetScopedName());

  msgs::Model msg;
  actor->FillMsg(msg);
  this->dataPtr->modelPub->Publish(msg);

  return actor;
}

//////////////////////////////////////////////////
RoadPtr World::LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent)
{
  RoadPtr road(new Road(_parent));
  road->Load(_sdf);
  return road;
}

//////////////////////////////////////////////////
void World::LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent)
{
  if (_sdf->HasElement("light"))
  {
    sdf::ElementPtr childElem = _sdf->GetElement("light");
    while (childElem)
    {
      this->LoadLight(childElem, _parent);

      childElem = childElem->GetNextElement("light");
    }
  }

  if (_sdf->HasElement("model"))
  {
    sdf::ElementPtr childElem = _sdf->GetElement("model");

    while (childElem)
    {
      this->LoadModel(childElem, _parent);

      // TODO : Put back in the ability to nest models. We should do this
      // without requiring a joint.

      childElem = childElem->GetNextElement("model");
    }
  }

  if (_sdf->HasElement("actor"))
  {
    sdf::ElementPtr childElem = _sdf->GetElement("actor");

    while (childElem)
    {
      this->LoadActor(childElem, _parent);

      childElem = childElem->GetNextElement("actor");
    }
  }

  if (_sdf->HasElement("road"))
  {
    sdf::ElementPtr childElem = _sdf->GetElement("road");
    while (childElem)
    {
      this->LoadRoad(childElem, _parent);
      childElem = childElem->GetNextElement("road");
    }
  }
}

//////////////////////////////////////////////////
unsigned int World::GetModelCount() const
{
  return this->dataPtr->models.size();
}

//////////////////////////////////////////////////
unsigned int World::LightCount() const
{
  return this->dataPtr->lights.size();
}

//////////////////////////////////////////////////
ModelPtr World::GetModel(unsigned int _index) const
{
  if (_index >= this->dataPtr->models.size())
  {
    gzerr << "Given model index[" << _index << "] is out of range[0.."
          << this->dataPtr->models.size() << "]\n";
    return ModelPtr();
  }

  return this->dataPtr->models[_index];
}

//////////////////////////////////////////////////
Model_V World::GetModels() const
{
  return this->dataPtr->models;
}

//////////////////////////////////////////////////
Light_V World::Lights() const
{
  return this->dataPtr->lights;
}

//////////////////////////////////////////////////
void World::ResetTime()
{
  this->dataPtr->simTime = common::Time(0);
  this->dataPtr->pauseTime = common::Time(0);
  this->dataPtr->startTime = common::Time::GetWallTime();
  this->dataPtr->realTimeOffset = common::Time(0);
  this->dataPtr->iterations = 0;

  if (this->IsPaused())
    this->dataPtr->pauseStartTime = this->dataPtr->startTime;

  // Signal a reset has occurred. The SensorManager listens to this event
  // to reset each sensor's last update time.
  event::Events::timeReset();
}

//////////////////////////////////////////////////
void World::ResetEntities(Base::EntityType _type)
{
  this->dataPtr->rootElement->Reset(_type);
}

//////////////////////////////////////////////////
void World::Reset()
{
  bool currentlyPaused = this->IsPaused();
  this->SetPaused(true);

  {
    boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);

    math::Rand::SetSeed(math::Rand::GetSeed());
    ignition::math::Rand::Seed(ignition::math::Rand::Seed());
    this->dataPtr->physicsEngine->SetSeed(math::Rand::GetSeed());

    this->ResetTime();
    this->ResetEntities(Base::BASE);
    for (auto &plugin : this->dataPtr->plugins)
    {
      plugin->Reset();
    }
    this->dataPtr->physicsEngine->Reset();

    // Signal a reset has occurred
    event::Events::worldReset();
  }

  this->SetPaused(currentlyPaused);
}

//////////////////////////////////////////////////
void World::OnStep()
{
  this->dataPtr->stepInc = 1;
}

//////////////////////////////////////////////////
void World::PrintEntityTree()
{
  // Initialize all the entities
  for (unsigned int i = 0; i < this->dataPtr->rootElement->GetChildCount(); i++)
    this->dataPtr->rootElement->GetChild(i)->Print("");
}

//////////////////////////////////////////////////
gazebo::common::Time World::GetSimTime() const
{
  return this->dataPtr->simTime;
}

//////////////////////////////////////////////////
void World::SetSimTime(const common::Time &_t)
{
  this->dataPtr->simTime = _t;
}

//////////////////////////////////////////////////
gazebo::common::Time World::GetPauseTime() const
{
  return this->dataPtr->pauseTime;
}

//////////////////////////////////////////////////
gazebo::common::Time World::GetStartTime() const
{
  return this->dataPtr->startTime;
}

//////////////////////////////////////////////////
common::Time World::GetRealTime() const
{
  if (!util::LogPlay::Instance()->IsOpen())
  {
    if (this->dataPtr->pause)
    {
      return (this->dataPtr->pauseStartTime - this->dataPtr->startTime) -
             this->dataPtr->realTimeOffset;
    }
    else
    {
      return (common::Time::GetWallTime() - this->dataPtr->startTime) -
             this->dataPtr->realTimeOffset;
    }
  }
  else
    return this->dataPtr->logRealTime;
}

//////////////////////////////////////////////////
bool World::IsPaused() const
{
  return this->dataPtr->pause;
}

//////////////////////////////////////////////////
void World::SetPaused(bool _p)
{
  if (this->dataPtr->pause == _p)
    return;

  {
    boost::recursive_mutex::scoped_lock lk(*this->dataPtr->worldUpdateMutex);
    this->dataPtr->pause = _p;
  }

  if (_p)
  {
    // This is also a good time to clear out the logging buffer.
    util::LogRecord::Instance()->Notify();

    this->dataPtr->pauseStartTime = common::Time::GetWallTime();
  }
  else
  {
    this->dataPtr->realTimeOffset += common::Time::GetWallTime() -
                                     this->dataPtr->pauseStartTime;
  }

  event::Events::pause(_p);
}

//////////////////////////////////////////////////
void World::OnFactoryMsg(ConstFactoryPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->factoryMsgs.push_back(*_msg);
}

//////////////////////////////////////////////////
void World::OnControl(ConstWorldControlPtr &_data)
{
  if (_data->has_pause())
    this->SetPaused(_data->pause());

  if (_data->has_step())
    this->OnStep();

  if (_data->has_multi_step())
  {
    // stepWorld is a blocking call so set stepInc directly so that world stats
    // will still be published
    this->SetPaused(true);
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);
    this->dataPtr->stepInc = _data->multi_step();
  }

  if (_data->has_seed())
  {
    math::Rand::SetSeed(_data->seed());
    ignition::math::Rand::Seed(_data->seed());
    this->dataPtr->physicsEngine->SetSeed(_data->seed());
  }

  if (_data->has_reset())
  {
    this->dataPtr->needsReset = true;

    if (_data->reset().has_all() && _data->reset().all())
    {
      this->dataPtr->resetAll = true;
    }
    else
    {
      this->dataPtr->resetAll = false;

      if (_data->reset().has_time_only() && _data->reset().time_only())
        this->dataPtr->resetTimeOnly = true;

      if (_data->reset().has_model_only() && _data->reset().model_only())
        this->dataPtr->resetModelOnly = true;
    }
  }
}

//////////////////////////////////////////////////
void World::OnPlaybackControl(ConstLogPlaybackControlPtr &_data)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->playbackControlMsgs.push_back(*_data);
}

//////////////////////////////////////////////////
void World::ProcessPlaybackControlMsgs()
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->worldUpdateMutex);

  for (auto const &msg : this->dataPtr->playbackControlMsgs)
  {
    if (msg.has_pause())
      this->SetPaused(msg.pause());

    if (msg.has_multi_step())
    {
      // stepWorld is a blocking call so set stepInc directly so that
      // world stats will still be published
      this->SetPaused(true);
      this->dataPtr->stepInc += msg.multi_step();
    }

    if (msg.has_seek())
    {
      common::Time targetSimTime = msgs::Convert(msg.seek());
      util::LogPlay::Instance()->Seek(targetSimTime);
      this->dataPtr->stepInc = 1;
    }

    if (msg.has_rewind() && msg.rewind())
    {
      util::LogPlay::Instance()->Rewind();
      this->dataPtr->stepInc = 1;
      if (!util::LogPlay::Instance()->HasIterations())
        this->dataPtr->iterations = 0;
    }

    if (msg.has_forward() && msg.forward())
    {
      util::LogPlay::Instance()->Forward();
      this->dataPtr->stepInc = -1;
      this->SetPaused(true);
      // ToDo: Update iterations if the log doesn't have it.
    }
  }

  this->dataPtr->playbackControlMsgs.clear();
}

//////////////////////////////////////////////////
void World::OnRequest(ConstRequestPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->requestMsgs.push_back(*_msg);
}

//////////////////////////////////////////////////
void World::JointLog(ConstJointPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  int i = 0;
  for (; i < this->dataPtr->sceneMsg.joint_size(); i++)
  {
    if (this->dataPtr->sceneMsg.joint(i).name() == _msg->name())
    {
      this->dataPtr->sceneMsg.mutable_joint(i)->CopyFrom(*_msg);
      break;
    }
  }

  if (i >= this->dataPtr->sceneMsg.joint_size())
  {
    msgs::Joint *newJoint = this->dataPtr->sceneMsg.add_joint();
    newJoint->CopyFrom(*_msg);
  }
}

//////////////////////////////////////////////////
void World::OnModelMsg(ConstModelPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->modelMsgs.push_back(*_msg);
}

//////////////////////////////////////////////////
void World::BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity)
{
  if (_entity)
  {
    if (_entity->HasType(Entity::MODEL))
    {
      msgs::Model *modelMsg = _scene.add_model();
      boost::static_pointer_cast<Model>(_entity)->FillMsg(*modelMsg);
    }
    else if (_entity->HasType(Entity::LIGHT))
    {
      msgs::Light *lightMsg = _scene.add_light();
      boost::static_pointer_cast<physics::Light>(_entity)->FillMsg(*lightMsg);
    }

    for (unsigned int i = 0; i < _entity->GetChildCount(); ++i)
    {
      this->BuildSceneMsg(_scene, _entity->GetChild(i));
    }
  }
}

//////////////////////////////////////////////////
// void World::ModelUpdateTBB()
// {
//   tbb::parallel_for (tbb::blocked_range<size_t>(0,
//   this->dataPtr->models.size(), 10),
//       ModelUpdate_TBB(&this->dataPtr->models));
// }

//////////////////////////////////////////////////
void World::ModelUpdateSingleLoop()
{
  // Update all the models
  for (unsigned int i = 0; i < this->dataPtr->rootElement->GetChildCount(); i++)
    this->dataPtr->rootElement->GetChild(i)->Update();
}

//////////////////////////////////////////////////
void World::LoadPlugins()
{
  // Load the plugins
  if (this->dataPtr->sdf->HasElement("plugin"))
  {
    sdf::ElementPtr pluginElem = this->dataPtr->sdf->GetElement("plugin");
    while (pluginElem)
    {
      this->LoadPlugin(pluginElem);
      pluginElem = pluginElem->GetNextElement("plugin");
    }
  }

  // Load the plugins for all the models
  for (unsigned int i = 0; i < this->dataPtr->rootElement->GetChildCount(); i++)
  {
    if (this->dataPtr->rootElement->GetChild(i)->HasType(Base::MODEL))
    {
      ModelPtr model = boost::static_pointer_cast<Model>(
          this->dataPtr->rootElement->GetChild(i));
      // 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
    }
  }
}

//////////////////////////////////////////////////
void World::LoadPlugin(const std::string &_filename,
                       const std::string &_name,
                       sdf::ElementPtr _sdf)
{
  gazebo::WorldPluginPtr plugin = gazebo::WorldPlugin::Create(_filename,
                                                              _name);

  if (plugin)
  {
    if (plugin->GetType() != WORLD_PLUGIN)
    {
      gzerr << "World[" << this->GetName() << "] is attempting to load "
            << "a plugin, but detected an incorrect plugin type. "
            << "Plugin filename[" << _filename << "] name[" << _name << "]\n";
      return;
    }
    plugin->Load(shared_from_this(), _sdf);
    this->dataPtr->plugins.push_back(plugin);

    if (this->dataPtr->initialized)
      plugin->Init();
  }
}

//////////////////////////////////////////////////
void World::RemovePlugin(const std::string &_name)
{
  for (auto plugin = this->dataPtr->plugins.begin();
       plugin != this->dataPtr->plugins.end(); ++plugin)
  {
    if ((*plugin)->GetHandle() == _name)
    {
      this->dataPtr->plugins.erase(plugin);
      break;
    }
  }
}

//////////////////////////////////////////////////
void World::LoadPlugin(sdf::ElementPtr _sdf)
{
  std::string pluginName = _sdf->Get<std::string>("name");
  std::string filename = _sdf->Get<std::string>("filename");
  this->LoadPlugin(filename, pluginName, _sdf);
}

//////////////////////////////////////////////////
void World::ProcessEntityMsgs()
{
  boost::mutex::scoped_lock lock(this->dataPtr->entityDeleteMutex);

  for (auto &entityName : this->dataPtr->deleteEntity)
  {
    this->RemoveModel(entityName);
  }

  if (!this->dataPtr->deleteEntity.empty())
  {
    this->EnableAllModels();
    this->dataPtr->deleteEntity.clear();
  }
}

//////////////////////////////////////////////////
void World::ProcessRequestMsgs()
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  msgs::Response response;

  for (auto const &requestMsg : this->dataPtr->requestMsgs)
  {
    bool send = true;
    response.set_id(requestMsg.id());
    response.set_request(requestMsg.request());
    response.set_response("success");

    if (requestMsg.request() == "entity_list")
    {
      msgs::Model_V modelVMsg;

      for (unsigned int i = 0;
           i < this->dataPtr->rootElement->GetChildCount(); ++i)
      {
        BasePtr entity = this->dataPtr->rootElement->GetChild(i);
        if (entity->HasType(Base::MODEL))
        {
          msgs::Model *modelMsg = modelVMsg.add_models();
          ModelPtr model = boost::dynamic_pointer_cast<Model>(entity);
          model->FillMsg(*modelMsg);
        }
      }

      response.set_type(modelVMsg.GetTypeName());
      std::string *serializedData = response.mutable_serialized_data();
      modelVMsg.SerializeToString(serializedData);
    }
    else if (requestMsg.request() == "entity_delete")
    {
      boost::mutex::scoped_lock lock2(this->dataPtr->entityDeleteMutex);
      this->dataPtr->deleteEntity.push_back(requestMsg.data());
    }
    else if (requestMsg.request() == "entity_info")
    {
      BasePtr entity = this->dataPtr->rootElement->GetByName(requestMsg.data());
      if (entity)
      {
        if (entity->HasType(Base::MODEL))
        {
          msgs::Model modelMsg;
          ModelPtr model = boost::dynamic_pointer_cast<Model>(entity);
          model->FillMsg(modelMsg);

          std::string *serializedData = response.mutable_serialized_data();
          modelMsg.SerializeToString(serializedData);
          response.set_type(modelMsg.GetTypeName());
        }
        else if (entity->HasType(Base::LINK))
        {
          msgs::Link linkMsg;
          LinkPtr link = boost::dynamic_pointer_cast<Link>(entity);
          link->FillMsg(linkMsg);

          std::string *serializedData = response.mutable_serialized_data();
          linkMsg.SerializeToString(serializedData);
          response.set_type(linkMsg.GetTypeName());
        }
        else if (entity->HasType(Base::COLLISION))
        {
          msgs::Collision collisionMsg;
          CollisionPtr collision =
              boost::dynamic_pointer_cast<Collision>(entity);
          collision->FillMsg(collisionMsg);

          std::string *serializedData = response.mutable_serialized_data();
          collisionMsg.SerializeToString(serializedData);
          response.set_type(collisionMsg.GetTypeName());
        }
        else if (entity->HasType(Base::JOINT))
        {
          msgs::Joint jointMsg;
          JointPtr joint = boost::dynamic_pointer_cast<Joint>(entity);
          joint->FillMsg(jointMsg);

          std::string *serializedData = response.mutable_serialized_data();
          jointMsg.SerializeToString(serializedData);
          response.set_type(jointMsg.GetTypeName());
        }
      }
      else
      {
        response.set_type("error");
        response.set_response("nonexistent");
      }
    }
    else if (requestMsg.request().find("world_sdf") != std::string::npos)
    {
      this->UpdateStateSDF();

      sdf::ElementPtr newSdf(this->dataPtr->sdf);

      // FIXME: Handle scale better on the server so we don't need to unscale
      // SDF here. Issue #1825
      if (requestMsg.request() == "world_sdf_save")
      {
        // Substitute all models sdf with unscaled versions
        if (newSdf->HasElement("model"))
        {
          auto modelElem = newSdf->GetElement("model");
          while (modelElem)
          {
            auto name = modelElem->GetAttribute("name")->GetAsString();
            auto model = this->GetModel(name);
            if (model)
            {
              auto unscaled = model->UnscaledSDF()->Clone();

              modelElem->Copy(unscaled);
            }

            modelElem = modelElem->GetNextElement("model");
          }
        }
      }

      msgs::GzString msg;
      std::ostringstream stream;
      stream << "<?xml version='1.0'?>\n"
             << "<sdf version='" << SDF_VERSION << "'>\n"
             << newSdf->ToString("")
             << "</sdf>";

      msg.set_data(stream.str());

      std::string *serializedData = response.mutable_serialized_data();
      msg.SerializeToString(serializedData);
      response.set_type(msg.GetTypeName());
    }
    else if (requestMsg.request() == "scene_info")
    {
      this->dataPtr->sceneMsg.clear_model();
      this->dataPtr->sceneMsg.clear_light();
      this->BuildSceneMsg(this->dataPtr->sceneMsg, this->dataPtr->rootElement);

      std::string *serializedData = response.mutable_serialized_data();
      this->dataPtr->sceneMsg.SerializeToString(serializedData);
      response.set_type(this->dataPtr->sceneMsg.GetTypeName());
    }
    else if (requestMsg.request() == "spherical_coordinates_info")
    {
      msgs::SphericalCoordinates sphereCoordMsg;
      msgs::Set(&sphereCoordMsg, *(this->dataPtr->sphericalCoordinates));

      std::string *serializedData = response.mutable_serialized_data();
      sphereCoordMsg.SerializeToString(serializedData);
      response.set_type(sphereCoordMsg.GetTypeName());
    }
    else
      send = false;

    if (send)
    {
      this->dataPtr->responsePub->Publish(response);
    }
  }

  this->dataPtr->requestMsgs.clear();
}

//////////////////////////////////////////////////
void World::ProcessModelMsgs()
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  for (auto const &modelMsg : this->dataPtr->modelMsgs)
  {
    ModelPtr model;
    if (modelMsg.has_id())
      model = this->GetModelById(modelMsg.id());
    else
      model = this->GetModel(modelMsg.name());

    if (!model)
      gzerr << "Unable to find model["
            << modelMsg.name() << "] Id[" << modelMsg.id() << "]\n";
    else
    {
      model->ProcessMsg(modelMsg);

      // May 30, 2013: The following code was removed because it has a
      // major performance impact when dragging complex object via the GUI.
      // This code also does not seem to be necessary, since can just
      // publish the incoming changes instead of a full model message. We
      // are leaving it temporarily in case we find a need for it.
      //
      // Let all other subscribers know about the change
      // msgs::Model msg;
      // model->FillMsg(msg);
      // // FillMsg fills the visual components from initial sdf
      // // but problem is that Visuals may have changed e.g. through ~/visual,
      // // so don't publish them to subscribers.
      // for (int i = 0; i < msg.link_size(); ++i)
      // {
      //   msg.mutable_link(i)->clear_visual();
      //   for (int j = 0; j < msg.link(i).collision_size(); ++j)
      //   {
      //     msg.mutable_link(i)->mutable_collision(j)->clear_visual();
      //   }
      // }

      this->dataPtr->modelPub->Publish(modelMsg);
    }
  }

  if (!this->dataPtr->modelMsgs.empty())
  {
    this->EnableAllModels();
    this->dataPtr->modelMsgs.clear();
  }
}

//////////////////////////////////////////////////
void World::ProcessLightModifyMsgs()
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  for (auto const &lightModifyMsg : this->dataPtr->lightModifyMsgs)
  {
    LightPtr light = this->Light(lightModifyMsg.name());

    if (!light)
    {
      gzerr << "Light [" << lightModifyMsg.name() << "] not found."
            << " Use topic ~/factory/light to spawn a new light." << std::endl;
      continue;
    }
    else
    {
      // Update in scene message
      for (int i = 0; i < this->dataPtr->sceneMsg.light_size(); ++i)
      {
        if (this->dataPtr->sceneMsg.light(i).name() == lightModifyMsg.name())
        {
          this->dataPtr->sceneMsg.mutable_light(i)->MergeFrom(lightModifyMsg);
          break;
        }
      }

      // Update light object
      light->ProcessMsg(lightModifyMsg);
    }
  }

  if (!this->dataPtr->lightModifyMsgs.empty())
  {
    this->dataPtr->lightModifyMsgs.clear();
  }
}

//////////////////////////////////////////////////
void World::ProcessLightFactoryMsgs()
{
  // LoadLight also publishes to ~/light/factory so copy light factory msgs to
  // avoid deadlock in OnLightFactory callback when trying to lock receiveMutex
  std::list<msgs::Light> lightFactoryMsgsCopy;
  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

    std::copy(this->dataPtr->lightFactoryMsgs.begin(),
              this->dataPtr->lightFactoryMsgs.end(),
              std::back_inserter(lightFactoryMsgsCopy));
    this->dataPtr->lightFactoryMsgs.clear();
  }

  for (auto const &lightFactoryMsg : lightFactoryMsgsCopy)
  {
    LightPtr light = this->Light(lightFactoryMsg.name());
    if (light)
    {
      continue;
    }
    else
    {
      // Add to world SDF
      sdf::ElementPtr lightSDF = msgs::LightToSDF(lightFactoryMsg);
      lightSDF->SetParent(this->dataPtr->sdf);
      lightSDF->GetParent()->InsertElement(lightSDF);

      // Create new light object
      this->LoadLight(lightSDF, this->dataPtr->rootElement);
    }
  }
}

//////////////////////////////////////////////////
void World::ProcessFactoryMsgs()
{
  std::list<sdf::ElementPtr> modelsToLoad, lightsToLoad;

  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
    for (auto const &factoryMsg : this->dataPtr->factoryMsgs)
    {
      this->dataPtr->factorySDF->Root()->ClearElements();

      if (factoryMsg.has_sdf() && !factoryMsg.sdf().empty())
      {
        // SDF Parsing happens here
        if (!sdf::readString(factoryMsg.sdf(), this->dataPtr->factorySDF))
        {
          gzerr << "Unable to read sdf string[" << factoryMsg.sdf() << "]\n";
          continue;
        }
      }
      else if (factoryMsg.has_sdf_filename() &&
               !factoryMsg.sdf_filename().empty())
      {
        std::string filename = common::ModelDatabase::Instance()->GetModelFile(
            factoryMsg.sdf_filename());

        if (!sdf::readFile(filename, this->dataPtr->factorySDF))
        {
          gzerr << "Unable to read sdf file.\n";
          continue;
        }
      }
      else if (factoryMsg.has_clone_model_name())
      {
        ModelPtr model = this->GetModel(factoryMsg.clone_model_name());
        if (!model)
        {
          gzerr << "Unable to clone model[" << factoryMsg.clone_model_name()
                << "]. Model not found.\n";
          continue;
        }

        this->dataPtr->factorySDF->Root()->InsertElement(
            model->GetSDF()->Clone());

        std::string newName = model->GetName() + "_clone";
        newName = this->UniqueModelName(newName);

        this->dataPtr->factorySDF->Root()->GetElement("model")->GetAttribute(
                                                                  "name")
            ->Set(newName);
      }
      else
      {
        gzerr << "Unable to load sdf from factory message."
              << "No SDF or SDF filename specified.\n";
        continue;
      }

      if (factoryMsg.has_edit_name())
      {
        BasePtr base =
            this->dataPtr->rootElement->GetByName(factoryMsg.edit_name());
        if (base)
        {
          sdf::ElementPtr elem;
          if (this->dataPtr->factorySDF->Root()->GetName() == "sdf")
            elem = this->dataPtr->factorySDF->Root()->GetFirstElement();
          else
            elem = this->dataPtr->factorySDF->Root();

          base->UpdateParameters(elem);
        }
      }
      else
      {
        bool isActor = false;
        bool isModel = false;
        bool isLight = false;

        sdf::ElementPtr elem = this->dataPtr->factorySDF->Root()->Clone();

        if (!elem)
        {
          gzerr << "Invalid SDF:";
          this->dataPtr->factorySDF->Root()->PrintValues("");
          continue;
        }

        if (elem->HasElement("world"))
          elem = elem->GetElement("world");

        if (elem->HasElement("model"))
        {
          elem = elem->GetElement("model");
          isModel = true;
        }
        else if (elem->HasElement("light"))
        {
          elem = elem->GetElement("light");
          isLight = true;
        }
        else if (elem->HasElement("actor"))
        {
          elem = elem->GetElement("actor");
          isActor = true;
        }
        else
        {
          gzerr << "Unable to find a model, light, or actor in:\n";
          this->dataPtr->factorySDF->Root()->PrintValues("");
          continue;
        }

        elem->SetParent(this->dataPtr->sdf);
        elem->GetParent()->InsertElement(elem);
        if (factoryMsg.has_pose())
        {
          elem->GetElement("pose")->Set(msgs::ConvertIgn(factoryMsg.pose()));
        }

        if (isActor)
        {
          ActorPtr actor = this->LoadActor(elem, this->dataPtr->rootElement);
          actor->Init();
        }
        else if (isModel)
        {
          // Make sure model name is unique
          auto entityName = elem->Get<std::string>("name");
          entityName = this->UniqueModelName(entityName);
          elem->GetAttribute("name")->Set(entityName);

          modelsToLoad.push_back(elem);
        }
        else if (isLight)
        {
          lightsToLoad.push_back(elem);
        }
      }
    }

    this->dataPtr->factoryMsgs.clear();
  }

  // Load models
  for (auto const &elem : modelsToLoad)
  {
    try
    {
      boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);

      ModelPtr model = this->LoadModel(elem, this->dataPtr->rootElement);
      if (model != nullptr)
      {
        model->Init();
        // 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 i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++)
          {
            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
      }
    }
    catch (...)
    {
      gzerr << "Loading model from factory message failed\n";
    }
  }

  // Load lights
  for (auto const &elem : lightsToLoad)
  {
    try
    {
      boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);

      LightPtr light = this->LoadLight(elem, this->dataPtr->rootElement);
      light->Init();
    }
    catch (...)
    {
      gzerr << "Loading light from factory message failed\n";
    }
  }
}

//////////////////////////////////////////////////
ModelPtr World::GetModelBelowPoint(const math::Vector3 &_pt)
{
  ModelPtr model;
  EntityPtr entity = this->GetEntityBelowPoint(_pt);

  if (entity)
    model = entity->GetParentModel();
  else
    gzerr << "Unable to find entity below point[" << _pt << "]\n";

  return model;
}

//////////////////////////////////////////////////
EntityPtr World::GetEntityBelowPoint(const math::Vector3 &_pt)
{
  std::string entityName;
  double dist;
  math::Vector3 end;

  end = _pt;
  end.z -= 1000;

  this->dataPtr->physicsEngine->InitForThread();
  this->dataPtr->testRay->SetPoints(_pt, end);
  this->dataPtr->testRay->GetIntersection(dist, entityName);
  return this->GetEntity(entityName);
}

//////////////////////////////////////////////////
void World::SetState(const WorldState &_state)
{
  this->SetSimTime(_state.GetSimTime());
  this->dataPtr->logRealTime = _state.GetRealTime();
  this->dataPtr->iterations = _state.GetIterations();

  // Insertions (adapted from ProcessFactoryMsgs)
  auto insertions = _state.Insertions();
  for (auto const &insertion : insertions)
  {
    this->dataPtr->factorySDF->Root()->ClearElements();

    std::stringstream sdfStr;
    sdfStr << "<sdf version='" << SDF_VERSION << "'>"
           << insertion
           << "</sdf>";

    // SDF Parsing happens here
    if (!sdf::readString(sdfStr.str(), this->dataPtr->factorySDF))
    {
      gzerr << "Unable to read sdf string[" << insertion << "]" << std::endl;
      continue;
    }

    // Get entity being inserted
    bool isModel = false;
    bool isLight = false;

    auto elem = this->dataPtr->factorySDF->Root()->Clone();

    if (!elem)
    {
      gzerr << "Invalid SDF:" << std::endl;
      this->dataPtr->factorySDF->Root()->PrintValues("");
      continue;
    }

    if (elem->HasElement("world"))
      elem = elem->GetElement("world");

    if (elem->HasElement("model"))
    {
      elem = elem->GetElement("model");
      isModel = true;
    }
    else if (elem->HasElement("light"))
    {
      elem = elem->GetElement("light");
      isLight = true;
    }
    else
    {
      gzerr << "Unable to find a model or light in:" << std::endl;
      this->dataPtr->factorySDF->Root()->PrintValues("");
      continue;
    }

    elem->SetParent(this->dataPtr->sdf);
    elem->GetParent()->InsertElement(elem);

    if (isModel)
    {
      try
      {
        boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);

        ModelPtr model = this->LoadModel(elem, this->dataPtr->rootElement);
        if (model != nullptr)
        {
          model->Init();
          if (!util::LogPlay::Instance()->IsOpen())
            model->LoadPlugins();
        }
      }
      catch (...)
      {
        gzerr << "Loading model from world state insertion failed" << std::endl;
      }
    }
    else if (isLight)
    {
      try
      {
        boost::mutex::scoped_lock lock(this->dataPtr->factoryDeleteMutex);

        LightPtr light = this->LoadLight(elem, this->dataPtr->rootElement);
        light->Init();
      }
      catch (...)
      {
        gzerr << "Loading light from world state insertion failed." << std::endl;
      }
    }
  }

  // Model updates
  const ModelState_M modelStates = _state.GetModelStates();
  for (auto const &modelState : modelStates)
  {
    ModelPtr model = this->GetModel(modelState.second.GetName());
    if (model)
      model->SetState(modelState.second);
    else
      gzerr << "Unable to find model[" << modelState.second.GetName() << "]\n";
  }

  // Light updates
  const LightState_M lightStates = _state.LightStates();
  for (auto const &lightState : lightStates)
  {
    LightPtr light = this->Light(lightState.second.GetName());
    if (light)
      light->SetState(lightState.second);
    else
    {
      gzerr << "Unable to find light[" << lightState.second.GetName() << "]"
            << std::endl;
    }
  }

  // Deletions
  auto deletions = _state.Deletions();
  for (auto const &deletion : deletions)
  {
    // This works for models and lights
    this->RemoveModel(deletion);
  }
}

//////////////////////////////////////////////////
void World::InsertModelFile(const std::string &_sdfFilename)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  msgs::Factory msg;
  msg.set_sdf_filename(_sdfFilename);
  this->dataPtr->factoryMsgs.push_back(msg);
}

//////////////////////////////////////////////////
void World::InsertModelSDF(const sdf::SDF &_sdf)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  msgs::Factory msg;
  msg.set_sdf(_sdf.ToString());
  this->dataPtr->factoryMsgs.push_back(msg);
}

//////////////////////////////////////////////////
void World::InsertModelString(const std::string &_sdfString)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  msgs::Factory msg;
  msg.set_sdf(_sdfString);
  this->dataPtr->factoryMsgs.push_back(msg);
}

//////////////////////////////////////////////////
std::string World::StripWorldName(const std::string &_name) const
{
  if (_name.find(this->GetName() + "::") == 0)
    return _name.substr(this->GetName().size() + 2);
  else
    return _name;
}

//////////////////////////////////////////////////
void World::EnableAllModels()
{
  for (auto &model : this->dataPtr->models)
  {
    model->SetEnabled(true);
  }
}

//////////////////////////////////////////////////
void World::DisableAllModels()
{
  for (auto &model : this->dataPtr->models)
  {
    model->SetEnabled(false);
  }
}

//////////////////////////////////////////////////
void World::UpdateStateSDF()
{
  this->dataPtr->sdf->Update();
  sdf::ElementPtr stateElem = this->dataPtr->sdf->GetElement("state");
  stateElem->ClearElements();

  WorldState currentState(shared_from_this());
  currentState.FillSDF(stateElem);
}

//////////////////////////////////////////////////
void World::LogModelResources()
{
  if (!util::LogRecord::Instance()->RecordResources())
    return;

  std::set<std::string> modelNames;
  std::set<std::string> fileNames;
  auto addModelResource = [&](const std::string &_uri) {
    if (_uri.empty())
      return;

    const std::string modelPrefix = "model://";
    const std::string filePrefix = "file://";
    if (_uri.find(modelPrefix) == 0)
    {
      std::string modelName = _uri.substr(modelPrefix.size(),
                                          _uri.find("/", modelPrefix.size()) - modelPrefix.size());
      modelNames.insert(modelName);
    }
    else if (_uri.find(filePrefix) == 0 || _uri[0] == '/')
    {
      fileNames.insert(_uri);
    }
  };

  // record model resources if option is enabled.
  for (auto const &model : this->dataPtr->models)
  {
    sdf::ElementPtr modelElem = model->GetSDF();
    if (modelElem->HasElement("link"))
    {
      sdf::ElementPtr linkElem = modelElem->GetElement("link");
      while (linkElem)
      {
        if (linkElem->HasElement("visual"))
        {
          sdf::ElementPtr visualElem = linkElem->GetElement("visual");
          while (visualElem)
          {
            sdf::ElementPtr geomElem = visualElem->GetElement("geometry");
            if (geomElem->HasElement("mesh"))
            {
              const std::string meshUri = geomElem->GetElement("mesh")
                                              ->Get<std::string>("uri");
              if (!meshUri.empty())
              {
                addModelResource(meshUri);
              }
            }
            if (visualElem->HasElement("material"))
            {
              sdf::ElementPtr matElem = visualElem->GetElement("material");
              if (matElem->HasElement("script"))
              {
                sdf::ElementPtr scriptElem = matElem->GetElement("script");
                if (scriptElem->HasElement("uri"))
                {
                  std::string matUri = scriptElem->Get<std::string>("uri");
                  if (!matUri.empty())
                  {
                    addModelResource(matUri);
                  }
                }
              }
            }
            visualElem = visualElem->GetNextElement("visual");
          }
        }
        if (linkElem->HasElement("collision"))
        {
          sdf::ElementPtr collisionElem = linkElem->GetElement("collision");
          while (collisionElem)
          {
            sdf::ElementPtr geomElem = collisionElem->GetElement("geometry");
            if (geomElem->HasElement("mesh"))
            {
              const std::string meshUri = geomElem->GetElement("mesh")
                                              ->Get<std::string>("uri");
              if (!meshUri.empty())
              {
                addModelResource(meshUri);
              }
            }
            collisionElem = collisionElem->GetNextElement("collision");
          }
        }
        linkElem = linkElem->GetNextElement("link");
      }
    }
  }
  if (!util::LogRecord::Instance()->SaveModels(modelNames) ||
      !util::LogRecord::Instance()->SaveFiles(fileNames))
  {
    gzwarn << "Failed to save model resources during logging\n";
  }
}

//////////////////////////////////////////////////
bool World::OnLog(std::ostringstream &_stream)
{
  int bufferIndex = this->dataPtr->currentStateBuffer;
  // Save the entire state when its the first call to OnLog.
  if (util::LogRecord::Instance()->FirstUpdate())
  {
    this->dataPtr->sdf->Update();
    _stream << "<sdf version ='";
    _stream << SDF_VERSION;
    _stream << "'>\n";
    _stream << this->dataPtr->sdf->ToString("");
    _stream << "</sdf>\n";
  }
  else if (this->dataPtr->states[bufferIndex].size() >= 1)
  {
    {
      boost::mutex::scoped_lock lock(this->dataPtr->logBufferMutex);
      this->dataPtr->currentStateBuffer ^= 1;
    }
    for (auto const &worldState : this->dataPtr->states[bufferIndex])
    {
      _stream << "<sdf version='" << SDF_VERSION << "'>"
              << worldState
              << "</sdf>";
    }

    this->dataPtr->states[bufferIndex].clear();
  }

  // Logging has stopped. Wait for log worker to finish. Output last bit
  // of data, and reset states.
  if (!util::LogRecord::Instance()->Running())
  {
    boost::mutex::scoped_lock lock(this->dataPtr->logBufferMutex);

    // Output any data that may have been pushed onto the queue
    for (size_t i = 0;
         i < this->dataPtr->states[this->dataPtr->currentStateBuffer ^ 1].size();
         ++i)
    {
      _stream << "<sdf version='" << SDF_VERSION << "'>"
              << this->dataPtr->states[this->dataPtr->currentStateBuffer ^ 1][i]
              << "</sdf>";
    }

    for (size_t i = 0;
         i < this->dataPtr->states[this->dataPtr->currentStateBuffer].size();
         ++i)
    {
      _stream << "<sdf version='" << SDF_VERSION << "'>"
              << this->dataPtr->states[this->dataPtr->currentStateBuffer][i]
              << "</sdf>";
    }

    // Clear everything.
    this->dataPtr->states[0].clear();
    this->dataPtr->states[1].clear();
    this->dataPtr->stateToggle = 0;
    this->dataPtr->prevStates[0] = WorldState();
    this->dataPtr->prevStates[1] = WorldState();
  }

  this->LogModelResources();

  return true;
}

//////////////////////////////////////////////////
void World::ProcessMessages()
{
  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

    if ((this->dataPtr->posePub && this->dataPtr->posePub->HasConnections()) ||
        (this->dataPtr->poseLocalPub &&
         this->dataPtr->poseLocalPub->HasConnections()))
    {
      msgs::PosesStamped msg;

      // Time stamp this PosesStamped message
      msgs::Set(msg.mutable_time(), this->GetSimTime());

      if (!this->dataPtr->publishModelPoses.empty() ||
          !this->dataPtr->publishLightPoses.empty())
      {
        for (auto const &model : this->dataPtr->publishModelPoses)
        {
          std::list<ModelPtr> modelList;
          modelList.push_back(model);
          while (!modelList.empty())
          {
            ModelPtr m = modelList.front();
            modelList.pop_front();
            msgs::Pose *poseMsg = msg.add_pose();

            // Publish the model's relative pose
            poseMsg->set_name(m->GetScopedName());
            poseMsg->set_id(m->GetId());
            msgs::Set(poseMsg, m->GetRelativePose().Ign());

            // Publish each of the model's child links relative poses
            Link_V links = m->GetLinks();
            for (auto const &link : links)
            {
              poseMsg = msg.add_pose();
              poseMsg->set_name(link->GetScopedName());
              poseMsg->set_id(link->GetId());
              msgs::Set(poseMsg, link->GetRelativePose().Ign());
            }

            // add all nested models to the queue
            Model_V models = m->NestedModels();
            for (auto const &n : models)
              modelList.push_back(n);
          }
        }

        for (auto const &light : this->dataPtr->publishLightPoses)
        {
          msgs::Pose *poseMsg = msg.add_pose();

          // Publish the light's pose
          poseMsg->set_name(light->GetScopedName());
          // \todo Change to relative once lights can be attached to links
          // on the rendering side
          // \todo Hack: we use empty id to indicate it's pose of a light
          // Need to add an id field to light.proto
          // poseMsg->set_id(light->GetId());
          msgs::Set(poseMsg, light->GetWorldPose().Ign());
        }

        if (this->dataPtr->posePub && this->dataPtr->posePub->HasConnections())
          this->dataPtr->posePub->Publish(msg);
      }

      if (this->dataPtr->poseLocalPub &&
          this->dataPtr->poseLocalPub->HasConnections())
      {
        // rendering::Scene depends on this timestamp, which is used by
        // rendering sensors to time stamp their data
        this->dataPtr->poseLocalPub->Publish(msg);
      }
    }
    this->dataPtr->publishModelPoses.clear();
    this->dataPtr->publishLightPoses.clear();
  }

  {
    boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

    if (this->dataPtr->modelPub && this->dataPtr->modelPub->HasConnections())
    {
      if (!this->dataPtr->publishModelScales.empty())
      {
        for (auto const &model : this->dataPtr->publishModelScales)
        {
          std::list<ModelPtr> modelList;
          modelList.push_back(model);
          while (!modelList.empty())
          {
            ModelPtr m = modelList.front();
            modelList.pop_front();

            // Publish the model's scale
            msgs::Model msg;
            msg.set_name(m->GetScopedName());
            msg.set_id(m->GetId());
            msgs::Set(msg.mutable_scale(), m->Scale());

            // Not publishing for links for now

            // add all nested models to the queue
            Model_V models = m->NestedModels();
            for (auto const &n : models)
              modelList.push_back(n);

            this->dataPtr->modelPub->Publish(msg);
          }
        }
      }
    }
    this->dataPtr->publishModelScales.clear();
  }

  if (common::Time::GetWallTime() - this->dataPtr->prevProcessMsgsTime >
      this->dataPtr->processMsgsPeriod)
  {
    this->ProcessPlaybackControlMsgs();
    this->ProcessEntityMsgs();
    this->ProcessRequestMsgs();
    this->ProcessFactoryMsgs();
    this->ProcessModelMsgs();
    this->ProcessLightFactoryMsgs();
    this->ProcessLightModifyMsgs();
    this->dataPtr->prevProcessMsgsTime = common::Time::GetWallTime();
  }
}

//////////////////////////////////////////////////
void World::PublishWorldStats()
{
  this->dataPtr->worldStatsMsg.Clear();

  msgs::Set(this->dataPtr->worldStatsMsg.mutable_sim_time(),
            this->GetSimTime());
  msgs::Set(this->dataPtr->worldStatsMsg.mutable_real_time(),
            this->GetRealTime());
  msgs::Set(this->dataPtr->worldStatsMsg.mutable_pause_time(),
            this->GetPauseTime());

  this->dataPtr->worldStatsMsg.set_iterations(this->dataPtr->iterations);
  this->dataPtr->worldStatsMsg.set_paused(this->IsPaused());

  if (util::LogPlay::Instance()->IsOpen())
  {
    msgs::LogPlaybackStatistics logStats;
    msgs::Set(logStats.mutable_start_time(),
              util::LogPlay::Instance()->LogStartTime());
    msgs::Set(logStats.mutable_end_time(),
              util::LogPlay::Instance()->LogEndTime());

    this->dataPtr->worldStatsMsg.mutable_log_playback_stats()->CopyFrom(
        logStats);
  }

  if (this->dataPtr->statPub && this->dataPtr->statPub->HasConnections())
    this->dataPtr->statPub->Publish(this->dataPtr->worldStatsMsg);
  this->dataPtr->prevStatTime = common::Time::GetWallTime();
}

//////////////////////////////////////////////////
bool World::IsLoaded() const
{
  return this->dataPtr->loaded;
}

//////////////////////////////////////////////////
void World::PublishModelPose(physics::ModelPtr _model)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

  // Only add if the model name is not in the list
  this->dataPtr->publishModelPoses.insert(_model);
}

//////////////////////////////////////////////////
void World::PublishModelScale(physics::ModelPtr _model)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

  // Only add if the model name is not in the list
  this->dataPtr->publishModelScales.insert(_model);
}

//////////////////////////////////////////////////
void World::PublishLightPose(const physics::LightPtr _light)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);

  // Only add if the light name is not in the list
  this->dataPtr->publishLightPoses.insert(_light);
}

//////////////////////////////////////////////////
void World::LogWorker()
{
  boost::mutex::scoped_lock lock(this->dataPtr->logMutex);

  WorldPtr self = shared_from_this();
  this->dataPtr->logPrevIteration = this->dataPtr->iterations;

  GZ_ASSERT(self, "Self pointer to World is invalid");

  // Init the prevUnfilteredState
  this->dataPtr->prevUnfilteredState.Load(self);

  while (!this->dataPtr->stop)
  {
    // get unfiltered world state
    WorldState unfilteredState;
    {
      boost::mutex::scoped_lock dLock(this->dataPtr->entityDeleteMutex);
      unfilteredState.Load(self);
    }

    // compute world state diff and find out about insertions and deletions
    std::vector<std::string> insertions;
    std::vector<std::string> deletions;
    bool insertDelete = false;

    {
      WorldState unfilteredDiffState = unfilteredState -
                                       this->dataPtr->prevUnfilteredState;
      if (!unfilteredDiffState.IsZero())
      {
        insertions = unfilteredDiffState.Insertions();
        deletions = unfilteredDiffState.Deletions();
        insertDelete = !insertions.empty() || !deletions.empty();
      }
    }
    this->dataPtr->prevUnfilteredState = unfilteredState;

    // Throttle state capture based on log recording frequency.
    auto simTime = this->GetSimTime();
    if ((simTime - this->dataPtr->logLastStateTime >=
         util::LogRecord::Instance()->Period()) ||
        insertDelete)
    {
      int currState = (this->dataPtr->stateToggle + 1) % 2;

      std::string filterStr = util::LogRecord::Instance()->Filter();
      // compute diff for filtered states
      {
        boost::mutex::scoped_lock dLock(this->dataPtr->entityDeleteMutex);
        this->dataPtr->prevStates[currState].LoadWithFilter(self, filterStr);
      }
      WorldState diffState = this->dataPtr->prevStates[currState] -
                             this->dataPtr->prevStates[this->dataPtr->stateToggle];
      this->dataPtr->logPrevIteration = this->dataPtr->iterations;

      if (!diffState.IsZero() || insertDelete)
      {
        this->dataPtr->stateToggle = currState;
        {
          // Store the entire current state (instead of the diffState). A slow
          // moving link may never be captured if only diff state is recorded.
          boost::mutex::scoped_lock bLock(this->dataPtr->logBufferMutex);

          this->dataPtr->prevStates[currState].SetInsertions(insertions);
          this->dataPtr->prevStates[currState].SetDeletions(deletions);
          this->dataPtr->states[this->dataPtr->currentStateBuffer].push_back(
              this->dataPtr->prevStates[currState]);

          // Tell the logger to update, once the number of states exceeds 1000
          if (this->dataPtr->states[this->dataPtr->currentStateBuffer].size() >
              1000)
          {
            util::LogRecord::Instance()->Notify();
          }
        }
      }

      this->dataPtr->logLastStateTime = simTime;
    }

    this->dataPtr->logContinueCondition.notify_all();

    // Wait until there is work to be done.
    this->dataPtr->logCondition.wait(lock);
  }

  // Make sure nothing is blocked by this thread.
  this->dataPtr->logContinueCondition.notify_all();
}

/////////////////////////////////////////////////
uint32_t World::GetIterations() const
{
  return this->dataPtr->iterations;
}

//////////////////////////////////////////////////
void World::RemoveModel(const std::string &_name)
{
  boost::recursive_mutex::scoped_lock plock(
      *this->GetPhysicsEngine()->GetPhysicsUpdateMutex());
  boost::mutex::scoped_lock flock(this->dataPtr->factoryDeleteMutex);

  // Remove all the dirty poses from the delete entity.
  {
    for (auto entity = this->dataPtr->dirtyPoses.begin();
         entity != this->dataPtr->dirtyPoses.end(); ++entity)
    {
      if ((*entity)->GetName() == _name ||
          ((*entity)->GetParent() && (*entity)->GetParent()->GetName() == _name))
      {
        this->dataPtr->dirtyPoses.erase(entity++);
      }
      else
        ++entity;
    }
  }

  // Remove from SDF
  if (this->dataPtr->sdf->HasElement("model"))
  {
    sdf::ElementPtr childElem = this->dataPtr->sdf->GetElement("model");
    while (childElem && childElem->Get<std::string>("name") != _name)
      childElem = childElem->GetNextElement("model");
    if (childElem)
    {
      this->dataPtr->sdf->RemoveChild(childElem);
    }
  }

  if (this->dataPtr->sdf->HasElement("light"))
  {
    sdf::ElementPtr childElem = this->dataPtr->sdf->GetElement("light");
    while (childElem && childElem->Get<std::string>("name") != _name)
      childElem = childElem->GetNextElement("light");
    if (childElem)
    {
      this->dataPtr->sdf->RemoveChild(childElem);
    }
  }

  // remove objects in world
  {
    boost::recursive_mutex::scoped_lock lock(
        *this->GetPhysicsEngine()->GetPhysicsUpdateMutex());

    // Remove model object
    for (auto model = this->dataPtr->models.begin();
         model != this->dataPtr->models.end(); ++model)
    {
      if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
      {
        this->dataPtr->models.erase(model);
        this->dataPtr->rootElement->RemoveChild(_name);
        break;
      }
    }

    // Remove light object
    for (auto light = this->dataPtr->lights.begin();
         light != this->dataPtr->lights.end(); ++light)
    {
      if ((*light)->GetScopedName() == _name)
      {
        if ((*light)->GetParent())
        {
          // Avoid calling: this->dataPtr->rootElement->RemoveChild(_name);
          // which removes the first child it finds with _name, ignoring
          // entity type (model or light) and scoping of names,
          // e.g. In a world with "point" and "parent::point" entities,
          // removing "point" will remove "parent::child" if it's first in the
          // list
          (*light)->GetParent()->RemoveChild(*light);
        }
        this->dataPtr->lights.erase(light);
        break;
      }
    }

    // Find the light by name in the scene msg, and remove it.
    for (int i = 0; i < this->dataPtr->sceneMsg.light_size(); ++i)
    {
      if (this->dataPtr->sceneMsg.light(i).name() == _name)
      {
        this->dataPtr->sceneMsg.mutable_light()->SwapElements(i,
                                                              this->dataPtr->sceneMsg.light_size() - 1);
        this->dataPtr->sceneMsg.mutable_light()->RemoveLast();
        break;
      }
    }
  }

  // Cleanup the publishModelPoses list.
  {
    boost::recursive_mutex::scoped_lock lock2(*this->dataPtr->receiveMutex);
    for (auto model = this->dataPtr->publishModelPoses.begin();
         model != this->dataPtr->publishModelPoses.end(); ++model)
    {
      if ((*model)->GetName() == _name || (*model)->GetScopedName() == _name)
      {
        this->dataPtr->publishModelPoses.erase(model);
        break;
      }
    }
  }

  // Cleanup the publishLightPoses list.
  {
    boost::recursive_mutex::scoped_lock lock2(*this->dataPtr->receiveMutex);
    for (auto light : this->dataPtr->publishLightPoses)
    {
      if (light->GetName() == _name || light->GetScopedName() == _name)
      {
        this->dataPtr->publishLightPoses.erase(light);
        break;
      }
    }
  }
}

/////////////////////////////////////////////////
void World::OnLightMsg(ConstLightPtr & /*_msg*/)
{
  gzerr << "Topic ~/light deprecated, use ~/factory/light to spawn new lights "
        << "and ~/light/modify to modify existing lights." << std::endl;
}

/////////////////////////////////////////////////
void World::OnLightModifyMsg(ConstLightPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->lightModifyMsgs.push_back(*_msg);
}

/////////////////////////////////////////////////
void World::OnLightFactoryMsg(ConstLightPtr &_msg)
{
  boost::recursive_mutex::scoped_lock lock(*this->dataPtr->receiveMutex);
  this->dataPtr->lightFactoryMsgs.push_back(*_msg);
}

/////////////////////////////////////////////////
msgs::Scene World::GetSceneMsg() const
{
  return this->dataPtr->sceneMsg;
}

/////////////////////////////////////////////////
boost::mutex *World::GetSetWorldPoseMutex() const
{
  return this->dataPtr->setWorldPoseMutex;
}

/////////////////////////////////////////////////
bool World::GetEnablePhysicsEngine()
{
  return this->dataPtr->enablePhysicsEngine;
}

/////////////////////////////////////////////////
void World::EnablePhysicsEngine(bool _enable)
{
  this->dataPtr->enablePhysicsEngine = _enable;
}

/////////////////////////////////////////////////
void World::_AddDirty(Entity *_entity)
{
  GZ_ASSERT(_entity != nullptr, "_entity is nullptr");
  this->dataPtr->dirtyPoses.push_back(_entity);
}

/////////////////////////////////////////////////
void World::ResetPhysicsStates()
{
  for (auto &model : this->dataPtr->models)
    model->ResetPhysicsStates();
}

//////////////////////////////////////////////////
std::string World::UniqueModelName(const std::string &_name)
{
  std::string result = _name;

  int i = 0;
  while (this->GetModel(result))
    result = _name + "_" + std::to_string(i++);

  return result;
}

// Added by zhangshuai 2019.04.02 ----Begin
//////////////////////////////////////////////////
/// add for read the element of "distribution"
DistributionPtr World::GetDistribution()
{
  return this->distribution;
}

//////////////////////////////////////////////////
/// add for read the element of "gazeboLocalID"
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