gazebo_plugin

Plugin §

• API参考

Plugin的六个大类： §

• World参考1参考2
• Model参考
• Sensor
• System参考
• Visual
• GUI

基本操作：创建插件 §

• 基础教程：参考（ROS_INFO输出）
• 例程

插件的两种基本使用方法 §

类方法与函数 §

• WorldPlugin() World类插件的函数，在加载Load()前执行一次
• Load() 世界初始化时执行的操作
• 添加在每次仿真迭代步进时执行操作
  1. 在类中声明变量 event::ConnectionPtr updateConnection;
  2. 在类中添加回调函数（以函数名为OnUpdate()为例）

     void OnUpdate()
     {
         ROS_INFO("OnUpdate()\n");
     }

  3. 在Load()中绑定回调函数

  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                           std::bind(&EnvMod::OnUpdate, this));

• 非阻塞函数可能存在执行时的时延，例如world->RemoveModel(this->model->GetName())
  • 不会立即产生效果（需要一段时间删除）
  • 不会在模型被删除后结束（如随后创建了同一个模型，若中间间隔时间较短可能会被删除）

通过消息对环境进行控制 §

• 参考
• Custom messages(Github version)
• 与rostopic类似

世界插件World Plugin §

创建世界插件 §

1. 创建ROS包（Gazebo Plugin只支持cpp，必须创建ROS软件包） 包名为gazebo_tutorials（即插件名）

   cd ~/catkin_ws/src
   catkin_create_pkg gazebo_tutorials gazebo_ros roscpp
   

2. 在gazebo_tutorials/src/simple_world_plugin.cpp中添加插件的源代码

   #include <gazebo/common/Plugin.hh>
   #include <ros/ros.h>
    
   namespace gazebo
   {
   class WorldPluginTutorial : public WorldPlugin
   {
   public:
     WorldPluginTutorial() : WorldPlugin()
     {
     }
    
     void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
     {
       // Make sure the ROS node for Gazebo has already been initialized
       if (!ros::isInitialized())
       {
         ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
           << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
         return;
       }
    
       ROS_INFO("Hello World!");
     }
    
   };
   GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
   }

3. 修改CMakeLists.txt（cmake_minimum_required的版本应当更新了）

   cmake_minimum_required(VERSION 2.8.3)
   project(gazebo_tutorials)
   
   # Check for c++11 / c++0x support
   include(CheckCXXCompilerFlag)
   CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
   CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
   if(COMPILER_SUPPORTS_CXX11)
       set(CMAKE_CXX_FLAGS "-std=c++11")
   elseif(COMPILER_SUPPORTS_CXX0X)
       set(CMAKE_CXX_FLAGS "-std=c++0x")
   else()
       message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
   endif()
   
   # Load catkin and all dependencies required for this package
   find_package(catkin REQUIRED COMPONENTS
     roscpp
     gazebo_ros
   )
   
   # Depend on system install of Gazebo
   find_package(gazebo REQUIRED)
   
   link_directories(${GAZEBO_LIBRARY_DIRS})
   include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
   
   catkin_package(
     DEPENDS
       roscpp
       gazebo_ros
   )
   
   add_library(${PROJECT_NAME} src/simple_world_plugin.cpp)
   target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
   

4. 在package.xml的<export>标签中添加 <gazebo_ros plugin_path="${prefix}/../../lib" gazebo_media_path="${prefix}" />
5. 创建World文件，注意添加了<plugin>标签

   <?xml version="1.0" ?>
   <sdf version="1.4">
     <world name="default">
       <include>
         <uri>model://ground_plane</uri>
       </include>
    
       <include>
         <uri>model://sun</uri>
       </include>
    
       <!-- reference to your plugin -->
       <plugin name="gazebo_tutorials" filename="libgazebo_tutorials.so"/>
     </world>
   </sdf>

6. 创建启动文件Launch File

   <launch>
     <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
     <include file="$(find gazebo_ros)/launch/empty_world.launch">
       <arg name="world_name" value="$(find gazebo_tutorials)/worlds/hello.world"/>
       <!-- more default parameters can be changed here -->
     </include>
   </launch>

在世界中加入模型的三种方法 §

• 参考

直接读入模型 §

`world->InsertModelFile("model://your_model");

从包含SDF的字串中读入模型 §

通过消息读入模型并进行初始化 §

this->node = transport::NodePtr(new gazebo::transport::Node());
this->node->Init(_world->Name());
factPub = node->Advertise<msgs::Factory>("~/factory");
// Create the message
msgs::Factory msg;
msg.set_sdf_filename("model://your_model");
 
// Pose to initialize the model to
msgs::Set(msg.mutable_pose(),
    ignition::math::Pose3d(
      ignition::math::Vector3d(0.1, 0.2, 0.3),
      ignition::math::Quaterniond(0, 0, 0)));
 
// Send the message
factPub->Publish(msg);
 

在世界插件中获取模型的指针 §

`

• 查看当前世界中的模型：

  • 当前世界中模型的数量 this->world->ModelCount();
  • 获取当前世界中的模型列表 physics::Model_V list = this->world->Models();
  • 可以用序号访问某个模型，进行获取模型名称等操作： list[i]->GetName();

• 若需要等待模型载入参考

  1. 不能使用while(model==NULL)（无论是否使用ROS的sleep，都无法获取到载入的模型）
  2. 在类中声明变量

  event::ConnectionPtr add_entity_connection;
  bool boxReady = false;

  1. 在Load()中为变量绑定添加模型实体事件

  this->add_entity_connection = event::Events::ConnectAddEntity(
                                        std::bind(&EnvMod::addEntityEventCallback, 
                                        this, std::placeholders::_1));

  4. 在类中添加回调函数 传入的参数为模型名，触发回调函数时还无法直接在其内部获取模型指针，设置flag，在下一次触发更新函数（如绑定的OnUpdate()）时更新模型指针

  void addEntityEventCallback(const std::string &name)
  {
      // Check entity name...
      // Trigger initialization...
      printf("get: %s \n", name.c_str());
      if (name=="box")
          boxReady = true;
  }
  void OnUpdate()
  {
      if (this->box == NULL && rodReady==true)
        this->box = this->world->ModelByName("box");
  }

模型插件Model Plugin §

基本操作 §

• 获取某个模型的指针 physics::ModelPtr model = this->world->ModelByName("your_model");
  • 获取模型的名称 std::string name = GetName();
  • 获取模型的某个link的指针 physics::LinkPtr link = model->GetLink("your_link");
    • 获取某个link中心相对于世界坐标系的位置

    ignition::math::Pose3d pose = link->WorldPose()
    ignition::math::Vector3<double> position = pose.Pos();
    std::cout << position.X() << ", " << position.Y() << ", " << position.Z();

      获取质心位置的方法：`WorldCoGPose()`（center of gravity?）
    

获取运行时模型的姿态数据 §

• 获取模型位姿 `ignition::math::Pose3d pose = model->WorldPose();

修改运行时模型姿态 §

• 代码示例

  ignition::math::Pose3d model_pose = model->WorldPose();
  ignition::math::Vector3d new_vec(1.0, 2.0, 3.0);
  ignition::math::Quaterniond rot(model_pose.Rot());
  ignition::math::Pose3d new_pose(new_vec, rot);
  model->SetWorldPose(new_pose);

修改运行时模型的外形（碰撞外形） §

• 代码示例（以圆柱形为例）

  // Get link
  physics::LinkPtr link = model->GetLink("target_link");
  // Get link collision property
  physics::CollisionPtr collision = link->GetCollision("target_link_collision");
  physics::ShapePtr shape = collision->GetShape();
  int shape_type = collision->GetShapeType();
  boost::shared_ptr<physics::CylinderShape> cylinder = boost::dynamic_pointer_cast<physics::CylinderShape>(shape);
  // Get link visual property
  cylinder->SetSize(1.0, 2.0);

修改运行时模型的外形（视觉外形） §

• 参考model_visuals.cc，需要通过消息进行修改。

1. 使用gazebo::transport::Node()时需引用#include <gazebo/gazebo.hh>

2. 需要声明一个节点并进行初始化 需要声明一个publisher

   transport::NodePtr node;
   transport::PublisherPtr visPub;
    
   this->node = transport::NodePtr(new gazebo::transport::Node());
   this->node->Init(_world->Name());
   visPub = this->node->Advertise<msgs::Visual>("~/visual");
   visPub->WaitForConnection();
    

3. 假设模型的某个link（physics::LinkPtr link = model->GetLink("target_link");）的标签为<visual name="link_vis">，需要对消息进行初始化，声明消息并配置其名称和父节点名称

   msgs::Visual visualMsg = link->GetVisualMessage("link_vis");
   visualMsg.set_name(link->GetScopedName());
   visualMsg.set_parent_name(model->GetScopedName());

   另一种获取VisualMessage的方法：

   sdf::ElementPtr linkSDF = link->GetSDF();
   sdf::ElementPtr visualSDF = linkSDF->GetElement("visual");
   std::string visual_name_ = visualSDF->Get<std::string>("name");
   msgs::Visual visualMsg = link->GetVisualMessage(visual_name_);

4. 配置link的visual属性

   • 修改模型透明度 visualMsg.set_transparency(0.5);
   • 修改模型可见性 visualMsg.set_visible(false);
   • 修改模型外形: 对VisualMessage使用has_geometry会返回false，大概是因为实例化的VisualMessage中没有配置geometry。使用mutable_geometry创建新的外形，执行后应该会覆盖原属性？（因为配置了名称和父节点名称）

   // update shape
   msgs::Geometry *geomMsg = visualMsg.mutable_geometry();
   geomMsg->set_type(msgs::Geometry::CYLINDER);
   geomMsg->mutable_cylinder()->set_radius(properties[3]);
   geomMsg->mutable_cylinder()->set_length(properties[4]);

5. 发布消息 visPub->Publish(visualMsg);

视觉插件Visual Plugin §

• 参考：修改heatmap地形
• 采用传输库transport与message的方法（类似rostopic）
  • 传输库示例1修改link可见度
  • 示例2修改box scale
  • 示例3修改link材质和scale Jun’18
• 获取场景参考system_plugin
  • 需引用头文件#include <gazebo/rendering/rendering.hh>
  • ？rendering::ScenePtr scene = rendering::get_scene(world->Name()); 好像也可以没有world->Name()，参数留空 执行后指针不为空但是再执行其它方法程序会崩溃（如scene->GetVisual("ground_plane")或scene.Name()）

实践 §

修改模型（碰撞外形+视觉外形） §

• 思路一：创建一个世界插件，从世界插件中访问模型，分别修改其碰撞外形和视觉外形
• 思路二：分别创建一个模型插件和一个一个视觉插件？

如何用Gazebo搭建一个强化学习的训练场景 §

Ignition API §

常见错误 §

• 编译错误

  error: invalid use of incomplete type ‘class gazebo::physics::World’
  24 |     _world->InsertModelFile("model://box");
  

  未引用头文件<gazebo/physics/physics.hh>