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
";
  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 的对接等。