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

Posted 2023-03-18 Mr.Winter`

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了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 <gazebo/gazebo.hh>

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);