沉默杀手HomeArchivesTagsCategories
(二) autonomous_exploration在仿真环境的运行
|路径规划CMU的autonomous_exploration|
Word count: 1.1k|Reading time: 4 min

搭建了五种仿真环境，室内环境/校园/停车场/隧道以及森林。

  • 校园环境(340m x 340m): 根据卡内基梅隆大学的部分校园环境重建得到，包含一些上下坡以及盘绕的地形。

  • 室内环境(130m x 100m): 包含长且窄的走廊以及许多桌子/椅子等障碍物，其中还有一个护栏，由于其中间可以穿透的特性，会对机器人的感知（perception）模块增加挑战性。

  • 停车场(140m x 130m, 5层): 包含多层楼且有上下坡，会对机器人3D导航任务增加难度

  • 隧道(330m x 250m): 错综复杂的隧道构成的一个庞大的网格结构

  • 森林(150m x 150m): 包含无规律分布的树木以及几栋房子。

仿真平台暂时不支持车在崎岖路面上的行驶的动态模拟（车轮和路面的接触之类的）。

仿真环境的制作： 如果不需要用地图渲染rgb图像的话，把纯激光雷达点云地图导入`Cloud Compare`进行downsample和计算每个点的normal。然后将其保存成文件（比如ply格式），再用`meshlab`打开并重建成mesh。最后用meshlab把它存成dae格式的文件就能被加载进gazebo了。 ### Simulation Environments 进入`src/vehicle_simulator/launch`，运行不同环境的launch，比如 
1
roslaunch vehicle_simulator system_garage.launch
还可以换成`system_indoor.launch`等等 使用**Waypoint** button in RVIZ 进行导航。为了在Gazebo观察, 设置launch文件中的`gazebo_gui = true` 环境文件在`src/vehicle_simulator/meshes`. `preview`文件夹中的`pointcloud.ply`是地图的点云文件，可使用 CloudCompare and MeshLab 打开。 ## 运行出现的问题 在虚拟机下运行gazebo，得到 vmw_ioctl_command error Invalid argument错误 解决方法是在终端下设置环境变量为0：`export SVGA_VGPU10=0`， 再加入环境变量
打开gazebo报错 [Err] [REST.cc:205] Error in REST request

执行sudo gedit ~/.ignition/fuel/config.yaml， 然后将url : https://api.ignitionfuel.org 改为 url: https://api.ignitionrobotics.org



打开gazebo_ros出错.png

vehicle_simulator.launch

vehicle_simulator.launch除了Gazebo那堆东西外，还有一个vehicleSimulator节点，发布map ---> sensor转换。 发布话题:

  • gazebo/set_model_state [gazebo_msgs/ModelState]
  • registered_scan [sensor_msgs/PointCloud2]
  • state_estimation [nav_msgs/Odometry]
  • tf [tf2_msgs/TFMessage]

订阅话题:

  • cmd_vel (50Hz)， path following 模块，用于发速度命令
  • terrain_map [sensor_msgs/PointCloud2]
  • velodyne_points [sensor_msgs/PointCloud2]

激光是基于Velodyne VLP-16进行模拟的，话题registered_scan，类型sensor_msgs/PointCloud2，频率5Hz，坐标系是map。 订阅者：

  • terrainAnalysisExt
  • sensorScanGeneration
  • localPlanner
  • terrainAnalysis
  • visualizationTools

话题state_estimation其实就是里程计，频率200Hz，从map坐标系到sensor坐标系。但twist部分一直都是0

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
header: 
  seq: 51875
  stamp: 
    secs: 262
    nsecs: 430000000
  frame_id: "/map"
child_frame_id: "/sensor"
pose: 
  pose: 
    position: 
      x: 2.87540912628
      y: 0.187103256583
      z: 0.711781859398
    orientation: 
      x: 0.00766504419326
      y: 0.00128535269673
      z: 0.0567104408485
      w: 0.998360416315
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# 省略twist

sensor_scan_generation.launch

sensor_scan_generation.launch就是把这state_estimation 和 registered_scan这两个话题做ROS时间戳同步，将map坐标系的点云state_estimation转为sensor_at_scan坐标系的sensor_scan (5Hz)。发布二者的tf变化，但是转换的矩阵是左乘transformToMap，它来自里程计，没理解为什么。

Visualization Tools

可视化工具，将探索过程中的三个实验指标曲线进行可视化，通过matplotlib绘图显示出来。 包括探索体积，探索路程，每次规划的计算时间.

visualization_tools.launch包括realTimePlot.py 和 visualizationTools.cpp。 realTimePlot.py就是窗口程序Figure 1，后者就是节点visualizationTools：
Publications:

  • explored_areas [sensor_msgs/PointCloud2]
  • explored_volume [std_msgs/Float32]
  • overall_map [sensor_msgs/PointCloud2]
  • time_duration [std_msgs/Float32]
  • trajectory [sensor_msgs/PointCloud2]
  • traveling_distance [std_msgs/Float32]

Subscriptions:

  • clock [rosgraph_msgs/Clock]
  • registered_scan [sensor_msgs/PointCloud2]
  • runtime [unknown type]
  • state_estimation [nav_msgs/Odometry]

为了观察算法的能力。RVIZ的Panels->Displays可以选择overallMap, exploredAreas, and trajectory

窗口Figure 1实时地观察 explored volume, traveling distance, and algorithm runtime。 如果要绘制runtime曲线, 用户需要发消息到这个话题上

  • Runtime runtime，类型std_msgs::Float32

但是我运行程序时，这三个指标曲线都没出现。

运行的最后，recorded metrics保存在文件src/vehicle_simulator/log, 以及vehicle trajectory. 由于仿真中Gazebo占用CPU很大，所以不能做实际情况参考。 可以从官网下载最好的 human practice results