ROS从入门到精通2-5：Gazebo插件制作案例(带碰撞属性的多行人场景)

0 专栏介绍

本专栏旨在通过对ROS的系统学习，掌握ROS底层基本分布式原理，并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。

详情：《ROS从入门到精通》

---

1 Gazebo插件

在ROS从入门到精通2-2：机器人3D物理仿真——Gazebo中介绍过Gazebo是一款3D物理仿真器，支持机器人开发所需的机器人、传感器和环境模型，并通过其搭载的强大物理引擎产生高品质的图形画面，达到逼真的仿真结果。

借助Gazebo可以构建机器人运动仿真模型、现实世界各种场景的仿真模型、传感器仿真模型、为机器人模型添加现实世界的物理性质。

和ROS从入门到精通2-4：Rviz插件制作案例(以多点导航插件为例)中介绍的Rviz插件类似，Gazebo插件也是独立的实例，可以插入正在运行的ROS系统进行功能性补充，而无需修改系统的底层代码，同时Gazebo插件使用的库文件可以获取仿真对象的各种数据——如传感器、运动信息等，可以大大简化构建仿真环境的过程，在Gazebo定制化仿真开发中必不可少。

目前Gazebo插件主要可分为

• World
• Model
• Sensor
• System
• Visual
• GUI

基于实际应用场景选择不同的插件类型，本文以模型插件为例介绍Gazebo的构造过程。

2 插件制作流程

以一个最简版本的Gazebo插件编写为例，介绍制作流程

• 编写插件头文件

  #include 
  
  namespace gazebo
  {
    class WorldPluginTutorial : public WorldPlugin
    {
      public: WorldPluginTutorial() : WorldPlugin();
      public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
    };
  }
  

  其中gazebo/gazebo.hh文件包含了Gazebo物理仿真的通用核心功能函数，但它不包括一些用于实现特殊需求的函数集合，例如gazebo/physics/physics.hh、gazebo/rendering/rendering.hh、gazebo/sensors/sensors.hh等等，这些头文件要根据具体场景引入。所有的插件实现都必须在gazebo命名空间下才生效。

  所有插件都必须继承自一个特定的Gazebo插件类，例如这里继承自WorldPlugin，意思就是实现一个世界插件，六种类型请参加第一节。

  注意：任何一个插件必须继承基类的Load函数接口，这个接口接收SDF文件元素，以加载SDF文件中指定的属性。

• 编写插件源文件

  namespace gazebo
  {
    WorldPluginTutorial::WorldPluginTutorial(){
    	std::cout << "Hello World!\n" << std::endl;
    }  
    
    void WorldPluginTutorial::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf){}
    };
    GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
  }
  

  源文件中必须使用GZ_REGISTER_XXX_PLUGIN宏向模拟器注册插件。这个宏的唯一参数是插件类的名称。每个插件类型都有匹配的宏，例如这里世界插件就是GZ_REGISTER_WORLD_PLUGIN。

• 编译插件

  cmake_minimum_required(VERSION 3.0.2)
  project(hello_world)
  
  find_package(gazebo REQUIRED)
  include_directories(include)
  include_directories(SYSTEM
    ${catkin_INCLUDE_DIRS}
    ${GAZEBO_INCLUDE_DIRS}
  )
  
  link_directories(
    ${catkin_LIBRARY_DIRS}
    ${GAZEBO_LIBRARY_DIRS}
  )
  
  add_library(hello_world SHARED hello_world.c)
  target_link_libraries(hello_world ${GAZEBO_LIBRARIES})
  

• 使用插件

  编译后会在/devel/lib目录下生成插件的动态链接库libhello_world.so，在模型.world文件中编写形如

  <world name="default">
      <plugin name="hello_world" filename="libhello_world.so"/>
  world>
  

  的代码即可使用插件

3 案例：带碰撞属性的多行人场景

3.1 激活行人碰撞属性

在Load函数中加入

...
// Configure the links
for (const auto &link : actor_->GetLinks())
{
  // Init the links, which in turn enables collisions
  link->Init();

  if (scales.empty())
    continue;

  // Process all the collisions in all the links
  for (const auto &collision : link->GetCollisions())
  {
    auto name = collision->GetName();
    if (scales.find(name) != scales.end())
    {
      auto boxShape = boost::dynamic_pointer_cast<gazebo::physics::BoxShape>(collision->GetShape());
      // Make sure we have a box shape.
      if (boxShape)
        boxShape->SetSize(boxShape->Size() * scales[name]);
    }

    if (offsets.find(name) != offsets.end())
    {
      collision->SetInitialRelativePose(offsets[name] + collision->InitialRelativePose());
    }
  }
}
...

将遍历行人模型的各个连杆，并依次激活碰撞属性。激活碰撞属性的目的在于使行人变为实体模型，能够被激光雷达等传感器扫描到，否则其相当于“幽灵”，会产生穿墙等不合理行为

3.2 实时计算行人位姿

相比第二节的最简插件，这里要额外继承一个OnUpdate接口，实现每个仿真步对行人位姿的更新，否则行人会静止不动

void PedestrianSFMPlugin::OnUpdate(const common::UpdateInfo &_info) {
  // Time delta
  double dt = (_info.simTime - last_update_).Double();

  ignition::math::Pose3d actor_pose = actor_->WorldPose();

  // update closest obstacle
  this->handleObstacles();
  // update pedestrian around
  this->handlePedestrians();

  // Compute Social Forces
  sfm::SFM.computeForces(sfm_actor_, other_actors_);
  // Update model
  sfm::SFM.updatePosition(sfm_actor_, dt);

  ...

  actor_pose.Pos().X(sfm_actor_.position.getX());
  actor_pose.Pos().Y(sfm_actor_.position.getY());
  actor_pose.Pos().Z(1.0);
  actor_pose.Rot() = ignition::math::Quaterniond(1.5707, 0, yaw);

  // Distance traveled is used to coordinate motion with the walking
  double distance_traveled = (actor_pose.Pos() - actor_->WorldPose().Pos()).Length();

  actor_->SetWorldPose(actor_pose);
  actor_->SetScriptTime(actor_->ScriptTime() + distance_traveled * animation_factor_);
  
  last_update_ = _info.simTime;
}

3.3 实时发布行人状态

通过在Gazebo插件中构造ROS节点来发布行人的实时状态，便于Rviz可视化调试，以及一些算法测试工作

// Create the ROS node
if (!ros::isInitialized())
{
  int argc = 0;
  char **argv = NULL;
  ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
}
node_.reset(new ros::NodeHandle("gazebo_client"));

// topic publisher
pose_pub_ = node_->advertise<geometry_msgs::PoseStamped>("/" + actor_->GetName() + "/pose", 10);

geometry_msgs::PoseStamped current_pose;
current_pose.header.frame_id = "map";
current_pose.header.stamp = ros::Time::now();
current_pose.pose.position.x = sfm_actor_.position.getX();
current_pose.pose.position.y = sfm_actor_.position.getY();
current_pose.pose.position.z = 1.0;
tf2::Quaternion q;
q.setRPY(0, 0, sfm_actor_.yaw.toRadian());
tf2::convert(q, current_pose.pose.orientation);

pose_pub_.publish(current_pose);

4 可视效果

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系