Gazebo的系统插件与ROS的仿真时间

我们知道Gazebo和ROS各自有一套自己的时间体系。它们实际上都有两个时间概念：墙上时间和系统时间。所谓的墙上时间(wall-time)就相当于是我们的物理世界中的时间， 就好比挂在墙上的一块钟表，在没有调整的情况下它将按照物理时间一分一秒的走下去。而系统时间在ROS环境下称为ROS Time在Gazebo的环境下成为仿真时间(sim-time)。

对于我们而言，系统时间或者说仿真时间是主要关注的对象，墙上时间没有太大的研究意义。既然我们选择使用ROS和Gazebo一起联合仿真，那么就需要将两者的系统时间建立起联系， 保证Gazebo世界中流逝的时间与ROS系统解算所用的时间是一个概念。本文中，我们将简单的了解一下两个系统的时间体系， 然后对gazebo_demos做些必要的修改统一时间体系。

1. 时间体系简介

我们研究Gazebo仿真，主要是想有一个廉价的机器，能够在它上面实验各种算法。Gazebo的系统是很松散的，可以部署在一个集群上，完成一些复杂场景的仿真任务。 各个模块或者子系统之间可以通过TCP/IP形式的消息发布订阅机制完成安全的通信。它这种松散的或者说是松耦合的组织形式，使得我们可以通过编写一些插件来构建纯虚拟的或者实时仿真系统。

完全由计算机软件构建的纯虚拟仿真系统中，虚拟空间与物理空间之间没有任何信息交互，可以认为两者之间没有约束，所以有时被称为无约束仿真。 在实时仿真中只有部分对象是由计算机模拟的，而其他的对象或者数据都是由物理世界中的具体事物提供的，或者是真实的人在参与仿真互动。 前者通常被称为半实物仿真(hardware-in-the-loop)，后者称为人在回路仿真(man-in-the-loop)。

时间对于仿真系统而言是一个基本的概念，我们需要保证在仿真世界中时间的一致性。对于Gazebo这种涉及到多台机器甚至于实时仿真系统而言， 其时间体系就更加微妙。一般而言仿真中有三类时间：

• 自然时间: 指物理世界中客观存在的不断流逝的时间。也称为了真实时间或者实际时间，它是我们能够感知到但无法准确测量和控制的时间。
• 机器时间: 虽然自然时间无法准确测量，但还是要观测它的。机器时间实际上就是对自然时间的观测， 一般通过计算机主板上的晶振和锁相环等电子器件产生固定频率的时钟信号并在软件系统中计数来实现。其实我们一开始提到的墙上时间指的就是它。 这个时间与硬件特性有关，所以在一定程度上是不可控的。但是由于需要软件计数，所以我们还是可以校正它的，这个校正的过程也是很多系统中提到的时间同步机制。
• 仿真时间: 指仿真世界中靠系统驱动不断流逝的时间。至于仿真系统想要用什么样的方式驱动它，完全是由系统和仿真想定来控制的。

仿真世界中时间的一致性是指，在任一时刻所有的仿真模型都必须对仿真时间有一致的认同。对于单台仿真计算机而言这点似乎没有太大的问题， 但涉及到计算机集群就需要考虑不同机器之间的时间同步问题了。而进行实时仿真的时候，还需要处理仿真时间与自然时间之间的关系。我们暂且认为Gazebo已经搞定了不同时间系统之间的微妙关系。 它在SDF文件的physics标签下提供了一些字段可以调整仿真时间与机器时间的对应关系:

ROS系统的目标是采集传感器的数据、完成环境感知和决策、控制执行器完成任务。时间对于ROS系统而言更多的是驱动算法估计环境和系统的状态，实时地完成对机器人的控制。 所以当控制实际的机器人时，ROS系统应当使用机器时间来完成解算；进行仿真时，它应当采用仿真时间。这是ROS系统时间体系存在的一方面意义。

2. 统一时间体系

统一时间体系，无非就是要让ROS系统使用Gazebo的仿真时间，这一点只需要将ROS的运行参数"/use_sim_time"设置为true就可以了。这样ROS系统就会订阅一个"/clock"的主题来驱动ROS系统的时间体系。 我们只需要在Gazebo的插件中通过该主题不断的发布仿真时间，游戏就结束了。但是这样运行demo要敲很多指令有点麻烦，所以我想改成通过roslaunch一键运行。

我们先在gazebo_demos的根目录下创建了一个launch的子目录，并在其中编写如下的velodyne.launch文件。 我们在第3行中，通过param标签将ROS运行参数"/use_sim_time"置为true。第4行以ros系统的节点的形式运行了脚本velodyne_world.sh，在该脚本中将运行gazebo并加载世界文件。

        <?xml version="1.0"?>
        <launch>
            <param name="/use_sim_time" value="true" />
            <node name="gazebo" pkg="gazebo_demos" type="velodyne_world.sh" output="screen"></node>
        </launch>

为了能够适应ROS系统的节点概念，我们专门在gazebo_demos的根目录下又创建了一个scripts的子目录，写下了如下的脚本velodyne_world.sh。 其中第2和3行是为了获得脚本velodyne_world.sh所在目录的绝对路径。第5行运行gazebo加载世界文件velodyne.world。

        #!/bin/sh
        [ -L ${0} ] && SCRIPT_DIR=$(readlink ${0}) || SCRIPT_DIR=${0}
        SCRIPT_DIR=$(dirname ${SCRIPT_DIR})
        WORLD_DIR=${SCRIPT_DIR}/../worlds 
        gazebo ${WORLD_DIR}/velodyne.world -s `catkin_find --first-only libgazebo_demos_ros_plugin.so`

在运行gazebo的时候，我们给定了一个运行参数-s `catkin_find --first-only libgazebo_demos_ros_plugin.so`，用于加载一个系统插件。 我们将在这个插件中完成ROS系统的初始化工作，并通过主题"/clock"发布仿真时间。 实际上，我们完全可以在Velodyne的模型插件中完成这些操作。但这些操作实际都是在对仿真系统进行初始化， 与某个传感器的模型插件绑定在一起不是一个很好的选择，所以我们决定提供这样一个系统层级的插件ROSPlugin， 如下面的代码片段所示。

        namespace gazebo {
            class ROSPlugin : public SystemPlugin {
                // ...
                private: gazebo::physics::WorldPtr mWorld;              // 仿真世界对象
                private: event::ConnectionPtr mWorldCreatedConnection;  // 完成仿真世界构建之后的事件联系
                private: event::ConnectionPtr mUpdateConnection;        // 开始仿真更新的事件联系
                private: std::unique_ptr<ros::NodeHandle> mRosNode;     // ROS系统的句柄对象
                private: ros::Publisher mRosClockPub;                   // 用于发布主题"/clock"消息的发布器
                // ...
            };
            GZ_REGISTER_SYSTEM_PLUGIN(ROSPlugin)
        };

上面的代码片段列举了插件类ROSPlugin中的所有成员变量。在Gazebo中一个World就是一个独立的仿真世界，它有自己的仿真配置和时间体系。 对象mWorld将在Gazebo完成仿真世界的加载工作后的回调函数中完成初始化工作，我们也会在后面的仿真更新事件的回调函数中看到如何通过对象mWorld获取仿真时间。

下面是插件类ROSPlugin的Load函数，它将在加载插件的时候被运行，完成系统插件的初始化操作。与其它类型(world、model、sensor)的插件不同的是，该Load函数有两个输入参数argc和argv， 它们是从gazebo运行之初通过main函数一路传递过来的命令行信息。在函数的一开始先检查是否已经完成了ROS的初始化工作，若没有则通过argc和argv两个输入参数完成初始化。 接着我们构建ROS的系统句柄对象，并通过它注册发布"/clock"主题。最后通过Gazebo的接口注册完成构建仿真世界事件和开始更新仿真时间的回调函数OnWorldCreated和OnWorldUpdateBegin。

        public: void Load(int argc, char **argv) {
            if (!ros::isInitialized())
                ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler);

            this->mRosNode.reset(new ros::NodeHandle("~"));
            this->mRosClockPub = this->mRosNode->advertise("/clock", 10);

            mWorldCreatedConnection = event::Events::ConnectWorldCreated(boost::bind(&ROSPlugin::OnWorldCreated, this, _1));
            mUpdateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ROSPlugin::OnWorldUpdateBegin, this));
        }

如下面的代码所示，我们在回调函数OnWorldCreated中获取仿真世界对象，在OnWorldUpdateBegin通过mWorld获取仿真时间并填充ROS消息发布之。

        
        private: void OnWorldCreated(std::string world_name) {
            mWorld = gazebo::physics::get_world(world_name);
        }
        

        private: void OnWorldUpdateBegin() {
            common::Time curTime = mWorld->SimTime();
            rosgraph_msgs::Clock rosTime;
            rosTime.clock.fromSec(curTime.Double());
            mRosClockPub.publish(rosTime);
        }

我们给gazebo_demos根目录下的CMakeLists.txt中添加如下的语句，指引编译生成libgazebo_demos_ros_plugin.so。

        add_library(gazebo_demos_ros_plugin src/ros_plugin.cc)
        target_link_libraries(gazebo_demos_ros_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES})

通过如下的指令我们就可以运行velodyne的仿真demo了。右图是在一个新的终端中通过指令$ rostopic info /clock查看主题"/clock"的发布和订阅情况的。 可以看到，此时gazebo就已经发布了该主题，而且被ros系统订阅了。

        $ roscd gazebo_demos
        $ source ./setup.sh 
        $ roslaunch gazebo_demos velodyne.launch

3. 完

本文中，我们简单的了解了一下仿真系统中所涉及到的时间概念，了解了如何调整Gazebo中系统中的仿真时间步长、迭代频率以及仿真时间与真实时间之间的比例关系。 然后为了统一Gazebo和ROS系统的时间体系，方便的运行demo，我们提供了launch文件，并通过bash脚本对gazebo的运行指令进行了封装。