首页 关于
树枝想去撕裂天空 / 却只戳了几个微小的窟窿 / 它透出天外的光亮 / 人们把它叫做月亮和星星
目录

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标签下提供了一些字段可以调整仿真时间与机器时间的对应关系:

字段名称 说明
max_step_size 最大的仿真时间步长。因为Gazebo支持四种不同的物理引擎,simbody这样的引擎使用的是可变时间步长的求解器,所以需要指定最大的时间步长。 而Gazebo默认使用的ODE引擎,则是固定步长的求解器,该值就是实际的仿真时间步长。默认情况下为0.001秒,即完成一次仿真迭代,仿真时间流逝0.001秒。
real_time_update_rate 该字段定义了仿真器的迭代频率,默认是1000Hz。如果该字段为0或者计算机的实际算力不能够支持该字段所要求的更新频率,那么Gazebo将尽可能快速的进行仿真求解。
real_time_factor 该字段描述了仿真时间与机器时间之间类似于“天庭一日,地上一年”的比例关系。如果该字段小于1,则意味着仿真时间比机器时间慢。 此外字段max_step_size和real_time_update_rate的乘积决定了该比例系数的上限。

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的运行指令进行了封装。




Copyright @ 高乙超. All Rights Reserved. 京ICP备16033081号-1