Gazebo的仿真插件
Gazebo通过插件(Plugin)的机制实现灵活的扩展功能。它的插件实际上是一个个的动态链接库，使得我们可以通过C++接触到Gazebo的方方面面。 而插件又是相对独立的，开发人员可以方便共享，使用时也可以方便的插入系统或者从系统中移除。关于它的插件机制， 官方也有很详细的教程和API文档。
在Gazebo中，针对不同层级的扩展，一共定义了6种插件：World, Model, Sensor, System, Visual, GUI。 这里我们在上一篇文章中实现的激光传感器的基础上，介绍Model的插件。 其它类型的插件，我们会在Gazebo的专题中进行介绍。 在上一篇文章中我们已经创建了一个完整功能的激光传感器模型， 它可以探测前方障碍的距离，具有一个自由度可以实现360°的扫描。如果读者跳过了上一节内容， 可以从这里 或者官方下载模型。
一些准备工作
在开始编写插件之前，我们需要确定已经安装了Gazebo的开发文件。对于Ubuntu系统，不确定的话可以通过如下指令安装，根据自己机器上的Gazebo版本号修改其中的数字7。
$ sudo apt install libgazebo7-dev
为了方便说明，我们在"~/catkin_ws/src/learning_gazebo/"目录下创建一个子目录"velodyne_plugin"作为编写插件的工作空间。
$ cd ~/catkin_ws/src/learning_gazebo
$ mkdir velodyne_plugin
$ cd velodyne_plugin
1. 从代码到运行的插件之旅
作为一个示例，我们先实现一个Model插件打印与之关联的模型名称。整个过程会经过四个阶段：编写C++程序、创建CMake编译脚本、在SDF文件中插入插件、运行测试。
1.1 编写C++程序
首先让我们创建一个"velodyne_plugin.cc"的文件，并将如下的代码拷贝进去。在代码中有比较详细的注释这里不再做解释了。
#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
namespace gazebo
{
// 一个控制Velodyne传感器的插件，它是一个Model类型的插件，所以继承自ModelPlugin
class VelodynePlugin : public ModelPlugin
{
// 构造函数
public: VelodynePlugin() {}
// 在插入本插件的时候，Gazebo就会调用该函数。
// 该函数是一个虚函数，我们可以通过重载该函数来对插件进行一些初始化的操作。
// 输入参数_model是一个指向与本插件相关联的模型的指针。
// 输入参数_sdf则是一个指向本插件SDF元素的指针。
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
// 目前我们只是打印出关联模型的名称
std::cerr << "\n本插件与模型[" << _model->GetName() << "]相关联" << std::endl;
}
};
// 向Gazebo注册本插件
GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
#endif
1.2 创建CMake编译脚本
接着，我们创建一个"CMakeLists.txt"的文件，将如下的内容拷贝进去，并通过CMake工具编译插件。
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
# Find Gazebo
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
# Build our plugin
add_library(velodyne_plugin SHARED velodyne_plugin.cc)
target_link_libraries(velodyne_plugin ${GAZEBO_libraries})
然后创建一个build目录并在该目录下编译插件：
$ mkdir build
$ cd build
$ cmake ..
$ make
如果编译没有问题，我们就会在build目录下看到"libvelodyne_plugin.so"的一个动态链接库。
1.3 在SDF文件中插入插件
接下来，我们回到一开始的工作空间中，创建一个"velodyne.world"的SDF文件，并把如下的内容拷贝进去：
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include><uri>model://sun</uri></include>
<!-- A ground plane -->
<include><uri>model://ground_plane</uri></include>
<!-- A testing model that includes the Velodyne sensor model -->
<model name="my_velodyne">
<include><uri>model://velodyne_hdl32</uri></include>
<!-- Attach the plugin to this model -->
<plugin name="velodyne_control" filename="libvelodyne_plugin.so"/>
</model>
</world>
</sdf>
这个SDF文件描述了一个叫做"default"的world，其中有三个模型分别描述了光照、地面和velodyne HDL32激光传感器。
激光传感器命名为"my_velodyne"，插入了我们创建的插件"libvelodyne_plugin.so"。
1.4 运行测试
在运行之前我们需要确认一下系统能够找到"libvelodyne_plugin.so"，我们可以通过export指令把build的子目录添加到环境变量LD_LIBRARY_PATH中。其中`pwd`的作用是获取当前路径。
$ cd build
$ export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:`pwd`
$ gazebo ../velodyne.world
运行上述指令之后就可以看到如下的日志：
本插件与模型[my_velodyne]相关联
2. 更细致的控制
我们已经实现了一个基本的Gazebo插件，现在通过PID控制器来控制激光传感器的转速。重新打开velodyne_plugin.cc文件，在其中类VelodynePlugin中添加三个私有的变量。 其中model和joint两个变量分别保存我们控制的传感器模型和转动关节，pid则是关节PID控制器。
private:
physics::ModelPtr model;
physics::JointPtr joint;
common::PID pid;
重写成员函数Load()如下:
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
if (_model->GetJointCount() == 0) {
std::cerr << "没有正确装在velodyne模型\n";
return;
}
this->model = _model;
this->joint = _model->GetJoints()[0];
// 配置PID控制器, 比例增益为0.1.
this->pid = common::PID(0.1, 0, 0);
// 应用PID控制器
this->model->GetJointController()->SetVelocityPID(this->joint->GetScopedName(), this->pid);
// 设定关节的目标速度
this->model->GetJointController()->SetVelocityTarget(this->joint->GetScopedName(), 10.0);
}
重新编译运行之后，我们就可以看到传感器在旋转。如果修改一下上述Load()最后的目标速度，重新编译运行可以看到转速是有变化的。
这种修改速度的方法是很繁琐的，下面我们看一种相对比较灵活的方法，在加载的SDF文件中修改转速。重新打开velodyne.world文件，修改<plugin>标签如下：
<plugin name="velodyne_control" filename="libvelodyne_plugin.so">
<velocity>25</velocity>
</plugin>
然后在Load()函数中添加对SDF文件的解析，从中获取速度的参数：
double velocity = 0;
// 解析SDF文件，从中获取速度参数
if (_sdf->HasElement("velocity"))
velocity = _sdf->Get<double>("velocity");
// 设定关节的目标速度
this->model->GetJointController()->SetVelocityTarget(this->joint->GetScopedName(), velocity);
如此，我们可以修改SDF文件来改变传感器的扫描速度，不用重新编译插件了。但这只是方便了一点，我们还希望在仿真过程中动态地控制传感器。
在Gazebo的仿真环境中，也有类似ROS的消息发布和订阅模式的消息传递方法。我们可以在插件中订阅一个主题，它描述了传感器的速度。 我们需要修改速度时，就发布一个该主题的消息，插件接收到消息后就会做出相应的动作。这是一种很方便的通信控制方式。
首先我们定义一个函数用来修改传感器的实际速度：
// 设定Velodyne的转速
// 输入参数_vel定义了控制的目标转速
void SetVelocity(const double &_vel)
{
this->model->GetJointController()->SetVelocityTarget(this->joint->GetScopedName(), _vel);
}
接下来我们要实现一个消息传递机制，先定义两个私有变量分别记录新建的节点和订阅器：
private:
transport::NodePtr node;
transport::SubscriberPtr sub;
然后在Load()函数中实例化这两个对象：
this->node = transport::NodePtr(new transport::Node());
this->node->Init(this->model->GetWorld()->GetName());
// 订阅主题名字
std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";
// 订阅主题，并注册回调函数OnMsg
this->sub = this->node->Subscribe(topicName, &VelodynePlugin::OnMsg, this);
我们在回调函数中解析接收到的消息，将其中的速度数据作为参数调用SetVelocity()。
// 处理接收的消息
// 输入参数_msg记录了接收到的消息内容，是一个Vector3的消息，这里只用到其中的x分量。
private: void OnMsg(ConstVector3dPtr &_msg)
{
this->SetVelocity(_msg->x());
}
此外我们还要包含两个头文件：
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
至此，我们对插件的修改就告一段落了，修改后的C++代码可以从这里下载。但是编译运行之后看不出有什么作用，
我们还需要实现一个消息的发布器，以后每次需要修改速度的时候就通过这个发布器发送一条消息。为此，我们再创建一个C++的程序vel.cc，
读者可以从这里下载，这里不再列出了，其中的主要内容就是通过Gazebo的API发布一个"~/my_velodyne/vel_cmd"的主题，
这也正是我们模型插件订阅的主题。
接下来，我们对CMakeLists.txt做出一些简单的修改，添加编译vel.cc的规则：
# Build the stand-alone test program
add_executable(vel vel.cc)
if (${gazebo_VERSION_MAJOR} LESS 6)
include(FindBoost)
find_package(Boost ${MIN_BOOST_VERSION} REQUIRED system filesystem regex)
target_link_libraries(vel ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
else()
target_link_libraries(vel ${GAZEBO_LIBRARIES})
endif()
编译之后运行仿真测试：
$ cd build
$ cmake ..
$ make
$ gazebo ../velodyne.world
我们再打开一个终端，切换到一开始的工作空间下的build子目录中，运行vel控制传感器的参数。
$ cd ~/catking_ws/src/learning_gazebo/velodyne_plugin/build
$ ./vel 2
我们可以从Gazebo的仿真环境中看到传感器以一个比较慢的速度转动，我们也可以尝试修改运行vel时的参数，可以看到传感器以不同的速度转动。
3. 通过ROS接口控制传感器
Gazebo的一个优势就是可以与ROS方便的连接，使得我们可以在真实的世界与模拟的世界之间自如的切换。 下面我们继续丰富激光传感器的插件功能，通过ROS来调整传感器的扫描速度。
首先，我们修改velodyne_plugin.cc文件(修改之后的文件可以从这里下载。)添加一些头文件用于访问ROS的接口， 如下左部分中的代码所示。接着在类VelodynePlugin中添加几个私有的成员变量， rosNode用于在ROS中注册一个节点，唯一地标识激光传感器插件。rosSub为一个ROS订阅器，用于订阅控制指令。 rosQueue和rosQueueThread分别是一个ROS回调函数队列和回调函数处理线程。看教程的意思是，没接收到一个消息都会创建一个线程处理。
|
|
接着，需要在函数Load()的最后添加一些ROS系统的初始化和注册的语句。主要是创建注册ROS节点，订阅ROS主题，同时注册两个回调函数。 OnRosMsg()用于响应接收消息，QueueThread则是一个ROS队列辅助线程的函数。
// 如果没有初始化ROS系统，则初始化.
if (!ros::isInitialized()) {
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
}
// 创建并注册ROS节点
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
// 订阅一个ROS主题.
ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(
"/" + this->model->GetName() + "/vel_cmd", // 主题名称
1,
boost::bind(&VelodynePlugin::OnRosMsg, this, _1), // 回调函数
ros::VoidPtr(), &this->rosQueue);
// 订阅主题
this->rosSub = this->rosNode->subscribe(so);
// ROS队列辅助线程
this->rosQueueThread = std::thread(std::bind(&VelodynePlugin::QueueThread, this));
然后，为类VelodynePlugin实现OnRosMsg()和QueueThread()两个回调函数：
// 处理ROS传来的消息
// _msg: 一个float数据，用于设定传感器转动速度
public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
{
this->SetVelocity(_msg->data);
}
// ROS处理消息的辅助函数
private: void QueueThread()
{
static const double timeout = 0.01;
while (this->rosNode->ok())
this->rosQueue.callAvailable(ros::WallDuration(timeout));
}
最后，我们需要对CMakeLists.txt做一些修改(修改之后的文件可以从这里下载)， 添加对ROS的访问。在CMakeLists.txt的开始添加roscpp和std_msgs两个包的依赖关系和搜索路径：
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
include_directories(${roscpp_INCLUDE_DIRS})
include_directories(${std_msgs_INCLUDE_DIRS})
为velodyne_plugin添加链接库：
target_link_libraries(velodyne_plugin ${GAZEBO_libraries} ${roscpp_LIBRARIES})
重新编译：
$ mkdir build
$ cd build
$ cmake ..
$ make
打开一个新的终端，以后台的形式运行roscore，前台的形式运行gazebo加载velodyne.world。
$ export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:`pwd`
$ roscore&
$ gazebo ../velodyne.world
再打开一个终端，通过rostopic的形式发布"/my_velodyne/vel_cmd"主题的消息，设置传感器转动速度，读者可以自行修改最后的参数调整转速。
rostopic pub /my_velodyne/vel_cmd std_msgs/Float32 10.0
4. 总结
在本文中，我们先介绍了创建、编译、插入、运行一个插件的全过程。然后利用Gazebo提供的各种API实现了比较灵活的控制激光传感器的转动速度。 最后介绍了Gazebo与ROS系统之间的一种交互方式。