从Velodyne的仿真插件开始

我们与Gazebo之间的交互主要是通过插件的形式进行的。而插件的编译和装载过程需要完成三个主要的步骤： (1) 在描述世界或者机器人模型的SDF文件中通过标签<plugin>指定插件的名称和动态连接库xxx.so文件。 (2) 用C++编写插件的接口类并根据不同的插件类型继承Gazebo提供的基础插件类。 (3) 编写CMake或者Make文件指引编译过程生成xxx.so，并通过环境变量${LD_LIBRARY_PATH}告知Linux系统xxx.so在哪个目录下。更详细的插件开发教程参见官方教程和Gazebo的仿真插件，这里不再赘述。

在机器人操作系统之ROS系列文章中，我们用SDF文件描述了激光雷达Velodyne HDL-32 LiDAR的结构。并为之添加了一个插件，使得我们可以通过ROS的接口控制雷达的扫描速度。这里可以找到根据Gazebo 11的API做了简单调整之后的demo源码。本文中，我们将在这个demo基础之上添加获取激光雷达的扫描数据，并输出ROS系统的LaserScan消息。

1. 订阅ROS主题控制雷达扫描速度

Velodyne HDL-32 LiDAR是一款32线的雷达。为了模拟多线激光雷达的扫描过程，我们在SDF文件中将一个32通道的"ray"类型的传感器绕x轴旋转90度放置。 "ray"是Gazebo原生提供的几种传感器之一，主要用于模拟激光测距。我们专门为这个激光雷达创建了插件类VelodynePlugin用于模拟雷达的各种特性，提供Gazebo与ROS之间的通信接口。由于我们写的是一个模型的插件，所以该类派生自ModelPlugin。

        physics::ModelPtr mModel;    
        physics::JointPtr mJoint;
        common::PID mPid;

为了能够控制激光雷达的扫描速度，我们在插件类VelodynePlugin中定义了三个私有的成员变量，如右侧代码所示。其中，指针mModel用于指示激光雷达模型在Gazebo中的对象；指针mJoint则指示了模型中唯一的旋转自由度，控制雷达的扫描速度实际上就是调整对象mJoint的转速；mPid则是一个PID控制器，用于控制对象mJoint的转速的。

上述的三个成员变量在Load函数中得到初始化，相关的代码片段如下所示。Load函数是Gazebo加载插件并具例化之后首先调用的函数，用于完成插件的各种初始化操作。该函数有两个参数，其中指针model指示了激光雷达模型在Gazebo中的对象实例，所以在代码片段的第3行中我们直接将之赋予mModel指针。指针sdf则指向了本插件的SDF元素，我们可以在SDF文件中添加一些配置信息，并通过这个指针加载到程序里。

        public: virtual void Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
            // ...
            this->mModel = model;
            this->mJoint = model->GetJoints()[0];
            this->mPid = common::PID(0.1, 0, 0);
            this->mModel->GetJointController()->SetVelocityPID(this->mJoint->GetScopedName(), this->mPid);
            // ...
        }

上面的代码片段的第4到6行中通过指针model获取了仿真模型中唯一的自由度，并为之创建了一个PID控制器，通过接口SetVelocityPID将该控制器与mJoint的速度绑定提供速度环控制。完成上述准备工作之后，我们就可以通过调用如下的接口函数SetVelocity来控制雷达的扫描速度。

        public: void SetVelocity(const double &vel) {
            this->mModel->GetJointController()->SetVelocityTarget(this->mJoint->GetScopedName(), vel);
        }

        std::unique_ptr<ros::NodeHandle> mRosNode;    
        ros::Subscriber mRosSub;
        ros::CallbackQueue mRosQueue;
        std::thread mRosQueueThread;

准备好上述控制接口之后，我们就可以通过订阅ROS的主题来接收速度控制指令，进而调用函数SetVelocity作出响应。但在这之前，我们还需要给插件添加一些关于ROS的基本对象，如右侧代码所示。其中，指针mRosNode指向一个ROS的句柄，是我们访问ROS接口的主要对象；对象mRosSub则是一个ROS的消息订阅器，这里被我们拿来订阅控制扫描速度的主题； ROS的回调队列mRosQueue和线程对象mRosQueueThread是ROS节点的运转机制，当回调任务需要被执行的时候都会先将之放置到队列mRosQueue中，再在线程对象中完成回调任务。

然后我们在Load函数中添加如下的配置语句，完成ROS系统的初始化工作，并实例化ROS句柄对象mRosNode。

        if (!ros::isInitialized()) {
            int argc = 0; char **argv = NULL;
            ros::init(argc, argv, "gazebo_demos", ros::init_options::NoSigintHandler);
        }
        this->mRosNode.reset(new ros::NodeHandle("gazebo_demos"));

接着通过SubscribeOptions订阅主题"/" + this->mModel->GetName() + "/vel_cmd"，并注册了回调函数OnVelCmdMsgFromROS，每当接收到了对应主题的消息，就会向回调任务队列mRosQueue中添加一个回调任务，我们将在ROS任务队列的线程函数中一一处理之。下面代码片段中第4行中完成了任务队列的处理线程的初始化工作，注册了线程函数RosQueueThread。

        ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>("/" + this->mModel->GetName() + "/vel_cmd", 1,
                    boost::bind(&VelodynePlugin::OnVelCmdMsgFromROS, this, _1), ros::VoidPtr(), &this->mRosQueue);
        this->mRosSub = this->mRosNode->subscribe(so);
        this->mRosQueueThread = std::thread(std::bind(&VelodynePlugin::RosQueueThread, this));

在下面的订阅消息回调函数OnVelCmdMsgFromROS中，我们从消息数据中获取雷达的目标速度，然后调用接口函数SetVelocity调整雷达转速。

        private: void OnVelCmdMsgFromROS(const std_msgs::Float32ConstPtr & msg) {
            this->SetVelocity(msg->data);
        }

而ROS任务队列的处理线程则在一个while循环中通过ROS的接口callAvailable处理消息回调任务。

        private: void RosQueueThread() {
            static const double timeout = 0.01;
            while (this->mRosNode->ok())
                this->mRosQueue.callAvailable(ros::WallDuration(timeout));
        }

我们可以通过如下的指令运行编译脚本build.sh编译插件，在编译之前我们需要确保已经source了脚本setup.sh加载必要的配置项。完成编译之后就可以运行gazebo并加载我们的插件。

        $ cd gazebo_demos
        $ source setup.sh
        $ cd demos/velodyne/
        $ ./build.sh
        $ gazebo velodyne.world

如果一切顺利，我们就可以通过rostopic直接控制雷达的转速了。

        $ rostopic pub /my_velodyne/vel_cmd std_msgs/Float32 10.0

2. 发布ROS主题输出激光扫描数据

到目前为止，我们已经能够控制雷达的转速了。但对于一个传感器而言，如果我们不能获取它的数据就有点尴尬了。下面，我们将发布一个ROS的主题，用来输出雷达的扫描数据。

要输出雷达的数据，就需要先从Gazebo的接口中获取"ray"的仿真数据。我们在用SDF文件模拟激光雷达的时候就见到过下面的两幅图。其中图(a)中蓝色的部分模拟了传感器发出的32束激光，深蓝色部分说明探测到了障碍，浅蓝色的部分则没有。在图(b)中，我们在传感器正前方放置了一个立方体，显然所有的探测光束都检测到了障碍，所以全部是深蓝色的。图中左上角还有一个对话框，是通过Gazebo的GUI界面上的菜单项Window->Topic Visualization打开的，修改其中的topic一栏订阅主题"~/my_velodyne/velodyne_hdl-32/top/sensor/scan"得到的。其中形象的展示了激光雷达的扫描数据。


图1(a). Gazebo下模型"velodyne_hdl-32"在发光	图1(b). Gazebo下模型"velodyne_hdl-32"传感器数据

也就是说，传感器"ray"在Gazebo中一直在发布一个主题为"~/my_velodyne/velodyne_hdl-32/top/sensor/scan"的消息。 Gazebo也使用了和ROS类似的订阅者和发布者模式进行通信，它们的使用套路基本都一样。都是构建节点、通过节点创建发布器或者接收器、通过回调函数来处理消息。所以在下面的代码片段中，我们给插件类VelodynePlugin添加了一些成员变量。其中，mGazeboNode是Gazebo用于通信的节点对象，mLaserScanSub是Gazebo的消息订阅器用来订阅主题"~/my_velodyne/velodyne_hdl-32/top/sensor/scan"，而队列mLaserScanQueue用于缓存需要发布的ROS消息。

        gazebo::transport::NodePtr mGazeboNode;
        gazebo::transport::SubscriberPtr mLaserScanSub;
        std::deque<sensor_msgs::LaserScan> mLaserScanQueue;

在Load函数中，我们增加如下的语句来对Gazebo的通信对象进行初始化。

        this->mGazeboNode = gazebo::transport::NodePtr(new gazebo::transport::Node());
        this->mGazeboNode->Init();
        mLaserScanSub = this->mGazeboNode->Subscribe("~/my_velodyne/velodyne_hdl-32/top/sensor/scan", &VelodynePlugin::OnLaserScanMsg, this);

在Gazebo的消息订阅回调函数OnLaserScanMsg中，我们将Gazebo的消息转换成ROS系统中的LaserScan消息。如下面的代码片段所示，这里我们主要强调消息的时间戳应当是Gazebo消息中记录的仿真时间，雷达的扫描数据是相对于"top"坐标系的。至于其它字段的填充工作参见源码。完成消息类型的转换之后，就将转换后的ROS消息放置到队列mLaserScanQueue中，在处理ROS回调任务的线程中发布出去。

        private: void OnLaserScanMsg(ConstLaserScanStampedPtr & msg) {
            sensor_msgs::LaserScan laser_msg;
            laser_msg.header.stamp = ros::Time(msg->time().sec(), msg->time().nsec());
            laser_msg.header.frame_id = "top";
            // 省略其它laser_msg字段的填充操作
            mLaserScanQueue.push_back(laser_msg);
        }

我们将ROS回调线程函数修改如下，在一个while循环中检查LaserScan消息队列是否为空，若非空则一一发布之。(貌似这样有资源竞争的问题，暂时先不管他，以后再来处理)。

        private: void RosQueueThread() {
            static const double timeout = 0.01;
            while (this->mRosNode->ok()) {
                while (!this->mLaserScanQueue.empty()) {
                    sensor_msgs::LaserScan &laser_msg = mLaserScanQueue.front();
                    this->mRosPub.publish(laser_msg);
                    mLaserScanQueue.pop_front();
                }
                this->mRosQueue.callAvailable(ros::WallDuration(timeout));
            }
        }

如果编译运行一切顺利，通过ROS下的可视化工具rviz就可以查看到激光雷达的扫描点了。读者可以把这个rviz配置文件下载下来，运行如下语句来查看ROS环境下的激光扫描数据，如下图所示。

        $ rviz -d rviz_config.rviz

3. 通过tf2发布坐标变换关系

        gazebo::common::Time now = this->mModel->GetWorld()->SimTime();
            double position = mJoint->Position();
            double c = std::cos(position);
            double s = std::sin(position);
            tf2::Matrix3x3 rot(c,  0,  s,
                               s,  0, -c,
                               0,  1,  0);
            tf2::Vector3 ori(0, 0, 0);
            tf2::Transform trans(rot, ori);
            geometry_msgs::TransformStamped stampedTrans;
            stampedTrans.transform = tf2::toMsg(trans);
            stampedTrans.header.frame_id = "base";
            stampedTrans.header.stamp = ros::Time(now.sec, now.nsec);
            stampedTrans.child_frame_id = "top";
            mTfBr->sendTransform(stampedTrans);

现在我们已经在ROS系统中看到了激光雷达的扫描数据，但这还不够，我们还要让rviz中的雷达数据跟着Gazebo的仿真同步转起来。这点可以借助ROS系统中的tf2工具来完成，我们只需要通过Gazebo的接口不断的查询关节的转角，将之转换成"top"相对于"base"的坐标变换关系，然后通过tf2的坐标变换发布器发布出去就可以了。

要做到这点，我们给插件类VelodynePlugin增加了字段tf2_ros::TransformBroadcaster *mTfBr;，这是一个坐标变换发布器的指针，同样需要在Load函数中完成初始化操作，只是需要注意这里我们没有使用智能指针，需要自己注意在析构函数中释放该对象。

        this->mTfBr = new tf2_ros::TransformBroadcaster();

接下来，我们在回调函数OnLaserScanMsg中添加右侧的代码片段，来获取Gazebo的仿真时间、关节的转角，进而构建tf坐标变换消息，通过刚刚构建的指针mTfBr发布出去。

其中我们需要尤其关注一下第5到7行构建的旋转矩阵rot，这个矩阵实际上是两个次旋转之后的结果。首先因为我们需要将"ray"绕x轴旋转90度放置，仿真多线激光扫描过程的效果。所以需要先与扫描光束固连的坐标系"top"先乘上一个矩阵$\begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix}$。接着由于光速在不断的绕着z轴转动，所以需要在乘上一个z轴的基础旋转矩阵$\begin{bmatrix} c & -s & 0 \\ s & c & 0 \\ 0 & 0 & 1 \end{bmatrix}$。两个矩阵相乘之后就得到了代码中所描述的姿态关系。

最后在15行通过指针mTfBr将构建好的坐标变换消息发布出去。此时如果运行仿真，并通过ROS的topic发布转速控制消息，就可以在rviz中看到雷达的扫描数据跟着Gazebo的仿真同步转动。

4. 完

在本文中，我们先简单回顾了一下Gazebo的插件介绍如何在Gazebo中订阅ROS的消息，进而实现控制雷达转速的功能。接着对插件进行扩展，获取了雷达扫描数据，并将之打包进一个ROS的消息中发布出来。此外，还了解了如何获取Gazebo的仿真时间、关节的位置，以及如何发布tf坐标变换消息。最终在可视化工具rviz中看到雷达的扫描数据跟着Gazebo的仿真同步转动。