Gazebo 機器人仿真流程

關於 Gazebo 仿真流程的核心代碼部分在 gazebo/gazebo 中，比如 gazebo.hh gazebo.cc 以及 physics/World.hh 等等

---

在 gazebo/examples/stand_alone/actuator/main.cc 有一個簡單的例子。依此例分析下 Gazebo 的仿真程序運行流程。

0、讀取一些配置相關的參數。

1、初始化 Gazebo

// Initialize gazebo.
gazebo::setupServer(_argc, _argv);

這里面就包括了：（1）建立 gazebo::Master ，這個應該是作為 ROS 節點的一類東西。（2）brief Setup the based gazebo system. 即 gazebo_shared::setup("server-", _argc, _argv, g_plugins) 。這里面包括了載入插件、初始化通訊、初始化插件等等。（3）載入傳感器。即 sensors::load() 。（4）載入/初始化物理引擎，即 gazebo::physics::load() 。（5）初始化傳感器，即 sensors::init() 。

2、載入場景（World）

// Load a world with two models: one actuated, one not
gazebo::physics::WorldPtr world = gazebo::loadWorld("../actuator_example.world");

主要的內容，還是依據 sdf 文件創建 physics 部分，即

world = gazebo::physics::create_world();
gazebo::physics::load_world(world, sdf->Root()->GetElement("world"));

gazebo::physics::init_world(world, rendering::update_scene_poses);

應該主要是載入模型，以及初始化。這些都是物理引擎部分。

3、一堆關於關節（joint）的操作。

4、運行仿真

gazebo::runWorld(world, sampleTimesteps);

這部分是仿真的重點。包含了機器人運動仿真的主要流程。其代碼實現為

/////////////////////////////////////////////////
void gazebo::runWorld(gazebo::physics::WorldPtr _world, unsigned int _iterations)
{
  if (!_world)
    gzerr << "World pointer is NULL\n";
  else
    _world->RunBlocking(_iterations);
}

隨后指向

void World::RunLoop();

進而是

void World::Step();

中間一頓操作，比如，發布當前狀態信息等。之后來到了

void World::Update();

這個才是 Gazebo 仿真的核心代碼。主要依次做了這么幾件事情：

（1）更新模型。（可能有一些模型需要依據自身的代碼進行狀態更新）

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

（2）碰撞對象的狀態更新

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

（3）進行物理引擎的計算及更新

// 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 plock(*this->Physics()->GetPhysicsUpdateMutex());

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

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

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

（4）發布碰撞/接觸信息

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

（5）結束更新，執行回調函數

event::Events::worldUpdateEnd();

猜測，一些事件處理需要在這里執行。比如，對於 contact information 需要什么樣的額外處理，等等

5、結束仿真

gazebo::shutdown();

---

這么看來，World 及 Physics 相關的部分，才是機器人運動仿真的核心部分。其余的多為特色功能、與 ROS 的對接等。