Gazebo的Topics

在介绍Gazebo的仿真插件的时候，我们简单提到Gazebo也使用了和ROS类似的订阅者和发布者模式进行通信。由发布者publisher提供一个主题topics，订阅者subscriber根据主题名称订阅消息，每当订阅者接收到发布者发布的主题消息，都会调用一个回调函数完成对订阅消息的处理。

Gazebo提供了一套接口，通过TCP/IP套接字来实现消息的发布与接收。它还使用Google Protobufs来解析和序列化消息。这些都封装在了Gazebo的库文件当中了，我们可以在C++程序中方便的调用。

本文中，我们将详细介绍Gazebo的仿真插件一文中略过的用于控制激光雷达转速的消息发布器vel，并对其进行扩展订阅激光的扫描数据。

1. 消息发布器

我们已经用SDF文件描述了激光雷达Velodyne HDL-32 LiDAR的模型，并为之创建了一个插件来控制雷达的转速。在《插件》一文中的更细致的控制一节中，我们构建了一个订阅器，订阅了一个"vel_cmd"的主题。该主题就是一个控制雷达转速的指令，每当接收到该主题的消息，插件都会调用回调函数OnMsg调整雷达转速。其中的node是Gazebo中的通信节点对象，这点我们可以对比ROS的Graph和Node，两者在功能上十分相似。

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

相应的，我们写了一个Gazebo的客户端来发布主题"vel_cmd"的消息。客户端不像插件那样，在Gazebo启动的时候被加载到系统中。我们可以在Gazebo启动之后，随时运行之。让我们先来看一下这个客户端的源文件"vel.cc"。下面的代码片段是其main函数，在函数的一开始先通过API接口注册成为gazebo的客户端。

        int main(int argc, char **argv) {
            gazebo::client::setup(argc, argv);

然后构建gazebo的通信节点，并完成初始化。

            gazebo::transport::NodePtr node(new gazebo::transport::Node());
            node->Init();

接着创建消息发布器，并耐心地等待着连接。

            gazebo::transport::PublisherPtr pub = node->Advertise("~/my_velodyne/vel_cmd");
            pub->WaitForConnection();

下面，我们就可以编写通信消息并发布之。这里需要注意，特定主题的消息是有特定格式的，订阅和发布的消息应当是相同类型的。

            gazebo::msgs::Vector3d msg;
            gazebo::msgs::Set(&msg, ignition::math::Vector3d(std::atof(argv[1]), 0, 0));
            pub->Publish(msg);

最后关闭客户端，通知gazebo释放与之相关的资源。

            gazebo::client::shutdown();
        }

相关的源码我们可以在Github上找到。我们将该仓库拉下来后，运行编译脚本。最后运行gazebo加载世界描述文件，就可以看到右图所示的场景。

        $ git clone https://github.com/gaoyichao/gazebo_demos.git
        $ cd gazebo_demos
        $ source setup.sh
        $ cd demos/velodyne
        $ ./build.sh
        $ gazebo velodyne.world

我们再打开一个终端，进入gazebo_demos的目录下，载入setup.sh后，运行客户端vel就可以在仿真环境中看到雷达转动了，如下面的代码所示。修改vel的运行参数可以调整雷达的转速。为了方便，可以将第2行的指令写在"~/.bashrc"中，这样每次打开新的终端就会自动装载必须的环境变量。

        $ cd gazebo_demos
        $ source setup.sh
        $ vel 10

总结一下，发布一个gazebo消息需要如下的四个步骤:

构建一个Gazebo通信节点，一般使用gazebo::transport::NodePtr指针类型记录节点对象。
根据新建的通信节点，调用成员函数Advertise发布主题，生成一个发布器对象。
构建符合主题消息类型的消息对象，并填充数据。
通过刚刚生成的发布器对象，将填充后的消息发布出去。

2. 消息订阅器

我们使用激光雷达主要是为了获得扫描数据，在模型文件中我们通过sensor标签，添加了"ray"传感器。在Gazebo的系统中，该传感器将通过主题"/gazebo/default/velodyne_hdl-32/top/sensor/scan"发布扫描数据。在Gazebo图形界面中选择菜单项Window->Topic Visualization，并在弹出的窗口中选择该主题点击OK后，就可以看到右图中左上角的窗口，形象的显示了扫描数据。

我们也可以在客户端中订阅该主题，来获取传感器数据。订阅一个主题，需要集齐下面这三颗龙珠:

Gazebo的通信节点: 它是消息发布和接收的逻辑对象。
消息订阅器: 它通常都是通过通信节点的成员函数Subscribe构建的。
回调函数: 每当订阅器接收到新的消息都会调用回调函数完成消息处理。

在刚才的发布器例子中，我们已经构建了通信节点。这里我们在main函数中添加一条语句，来获得第二颗龙珠。同时，在参数中注册了回调函数。

        gazebo::transport::SubscriberPtr sub = node->Subscribe("~/my_velodyne/velodyne_hdl-32/top/sensor/scan", cb);

下面的回调函数，只是简单的把接收到的消息打印出来。我们需要注意的是这个函数的参数列表，它是一个指针，指向了接收到的消息内容。

        void cb(ConstLaserScanStampedPtr & _msg)
        {
            std::cout << _msg->DebugString() << std::endl;
        }

另外，通过工具gz我们是可以查看当前Gazebo系统中的主题的。

        $ gz topic -l

3. 总结

Gazebo也使用了和ROS类似的订阅者和发布者模式进行通信，它们的使用套路基本都一样。都是构建节点、通过节点创建发布器或者接收器、通过回调函数来处理消息。

本文中，我们还简单的提到了Gazebo的客户端。实际上我们通过指令gazebo打开仿真环境时，运行了gzserver和gzclient两个部分。gzserver完成了仿真运算，gzclient则提供了GUI界面和编辑工具。