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

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

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

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

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

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

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

1

roslaunch vehicle_simulator system_garage.launch

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

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

检测高度难以调整

多线雷达安装在一定高度，地面上有个比较矮的障碍物，要在代价地图里添加。这显然涉及参数min_obstacle_height，要将其降低


只稍微降低了min_obstacle_height，虽然是检测到了，但是个环形，这跟多线雷达数据的形状一样，所以认为其实是检测到了地面。

这里发现代价地图能做的修改其实很少了，主要在LOAM和octomap_server了

检测距离不能太大

这说的是obstacle_range，如果太大，局部代价地图的窗口的四角都会出现障碍，不知是怎么添加进来的。

代价地图添加了太多不需要的点云数据

对于多线雷达，点云数据量比单线大了很多，在检测近处障碍的时候，计算sq_dist会得到很多点(虽然delta_z不是0)。单线雷达扫描的是一条线，多线其实是很多条线，但是代价地图是2D的，投影会有很多重复的，需要针对xy坐标去重，显著降低了效率，这又是costmap_2d不适合多线雷达的一个例子。

这种情况下，增大的网络占用已经不能忽略了，不能开Rviz，否则会大大影响到导航。

Gazebo对电脑显卡有一定要求，对Nvida显卡支持较好，对AMD的显卡支持较差，如果是AMD的显卡，复杂的世界模型一般加载不出来

利用spawn_model脚本向gazebo_ros节点（在主题中，空间名为 gazebo）发出服务请求，进而添加 URDF 到 Gazebo 中

1

rosrun gazebo_ros spawn_model -file `rospack find MYROBOT_description`/urdf/MYROBOT.urdf -urdf -x 0 -y 0 -z 1 -model MYROBOT

xacro转urdf

如果是kinetic，执行

1

rosrun xacro xacro.py robot1.xacro > robot1_processed.urdf

1
2
3

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order xacro.py is deprecated; please use xacro instead

如果是在Melodic，执行rosrun xacro xacro --inorder $(locate robot.urdf.xacro) > robot_inorder.urdf，结果得到

1

xacro: in-order processing became default in ROS Melodic. You can drop the option.

查看关节图

先执行sudo apt install -y liburdfdom-tools

1
2
3

check_urdf pr2.urdf
# 再打开转换的pdf
urdf_to_graphiz pr2.urdf

查看spawn_model的所有参数，可以运行：

1

rosrun gazebo_ros spawn_model -h

在 launch文件中，对于urdf可以这样编写：

1
2

<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find baxter_description)/urdf/baxter.urdf -urdf -z 1 -model baxter" />

对于xacro，这样编写:

1
2
3
4
5

<!-- Convert an xacro and put on parameter server -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find pr2_description)/robots/pr2.urdf.xacro" />

<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model pr2" />

URDF的报错原因：加了等标签，但没加具体内容。

rosrun gazebo_ros spawn_model -file robot.urdf -urdf -model robot

sdf: standard definition of world for Gazebo

1
2
3
4
5
6
7
8
9
10

<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch" >
  </include>

  <arg name='robot_urdf' default="$(find xacro)/xacro '$(find vehicle_simulator)/urdf/robot.urdf.xacro'" />
  <param name="robot_description" command="$(arg robot_urdf)" />
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_robot" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0.5"/>

  <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop" />
</launch>

Gazebo的gui参数经常是false，这是因为显示界面会显著加大资源占用

world文件可以直接改名称，不影响使用

兼容问题

加入camera仿真

Gazebo只给出了水平FOV, 竖直FOV会根据图片的宽和高自动计算。 Gazebo有函数double VerticalFOV() const，解释: The vertical FOV is calculated from width, height, hfov。有bool SetHorizontalFOV (double _hfov)函数，但是没有函数bool SetVerticalFOV (double _hfov)

差速模块

出现下面日志，说明差速控制模块正常加载。

1
2
3
4
5
6

[ INFO] [1720590402.172629168, 68.803000000]: Starting plugin DiffDrive(ns = //)
[ INFO] [1720590402.172729086, 68.803000000]: DiffDrive(ns = //): <rosDebugLevel> = na
[ INFO] [1720590402.173268546, 68.803000000]: DiffDrive(ns = //): <tf_prefix> = 
[ INFO] [1720590402.173831755, 68.803000000]: DiffDrive(ns = //): Try to subscribe to cmd_vel
[ INFO] [1720590402.175204165, 68.803000000]: DiffDrive(ns = //): Subscribe to cmd_vel
[ INFO] [1720590402.175652313, 68.803000000]: DiffDrive(ns = //): Advertise odom on odom

libgazebo_ros_gps_sensor.so

libgazebo_ros_bumper.so

libgazebo_ros_ray_sensor.so

安装: sudo apt-get install -y ros-melodic-plotjuggler-ros，运行 rosrun plotjuggler plotjuggler

张楫的CMU团队提出了几个上层规划算法，包括在未知环境中的探索算法和全局路径规划算法，以及全套算法的系统集成和扩展应用。目前开源的是autonomous_exploration, dsv_planner, tare_planner, far_planner。 Autonomous Exploration Development Environment里自带的local planner只是用来做局部路径规划和小范围避障的，目标点太远时容易卡在死胡同里。如果需要长距离的路径规划建议使用far planner。

TARE算法的论文获得了Best Paper Award and Best System Paper Award of RSS 2021。 CMU-OSU参加DARPA SubT 2021挑战赛用的是M-TARE的初级版本(没有开源)。地面机器人同样用的LOAM。

我目前只详细研究了autonomous_exploration的代码。

CMU提供的避障算法优势在于能实时高效地应对复杂环境,同时最大化到达目标点的概率。算法的主要思想在于尽可能地让计算在线下完成。具体来说，一个避障算法最耗时的计算一般在于检测机器人在未来时间点与环境发生碰撞的可能。我们的算法先通过离线生成一个庞大的轨迹库（trajectory library）来模拟机器人在未来一段时间内可能走过的轨迹。接下来对于所有轨迹覆盖的空间，我们计算其内部所有点（在一定解析度下）与所有轨迹发生碰撞的可能性。经过这样的离线计算，我们可以得到一个空间内3D点到轨迹的对应关系。在实时运行的过程中，一旦空间内的某个点上有障碍物，我们可以马上知道哪些轨迹将会受到影响。我们的算法会降低选择这些轨迹作为最终路径的可能性。因为大量的计算都在线下进行，线上运行的时候只需要实时选择无碰撞的轨迹，我们的算法可以在几毫秒之内规划出一条无碰撞而最接近目标点的路径。

ROS里经常出现一种情况：规划出的全局路径没有考虑车是否能过窄通道，结果车到了窄通道附近要么又重新规划到其他方向，要么强行过窄通道。本算法会优先规划路径到开阔的地方。 另外也就避免了全局路径的无限规划问题

测试平台

测试平台是室外机器人，车上用了velodyne-16激光雷达，IMU是xsens mti-200。速度只能达到2m/s，所以高速机器人尚需验证。工控机CPU为4.1GHz i7。 定位使用vLOAM的商业版，参考论文。 运行系统要求是18.04或者20.04，其实是因为仿真环境使用Gazebo7，如果不用Gazebo，在16.04上也可以使用。

使用registered scans的好处：

• 使scan数据和state estimation之间的processing对时间同步不太敏感
• 多个registered scans可以 stacked 到一起，从中提取丰富的几何环境信息

目前我实现的效果

由于LOAM的问题，z坐标不可靠，所以我在代码里都把里程计返回的z按0处理，so far只在没有上下坡的情况下测试过。目前基本可以正常导航，环境中可以有动态障碍，可以调整线速度和角速度，可以倒退，不会有路径混乱、花费时间长的情况。

成功的导航，提取码 2k50

目前在线速度0.5m/s的情况下，还是有一定误差的，亟待优化

视频
这个问题的本质是ROS的全局路径算法不考虑车体的轮廓，跟轮廓有关的避障交给了局部路径，DWA和TEB算法里都有footprintCost之类的函数，可以判断是否撞了障碍，但全局路径算法没有。 全局路径是把车当成了点，从所在栅格到目标栅格进行规划。这里开始规划出的路线就是这样，在走了一段时间被TEB认为不可行后，全局路径换成另一条，但是马上被认为不如之前的全局路径更优，又换回去了。如此循环，所以走不出去了。

但是要让全局路径考虑轮廓，会改动很大(即使不考虑动态障碍)，成本太高了。所以有必要考虑其他导航框架了。

暂时的解决方法只有设置中间点或者加虚拟墙

在ROS当中，原作者是不推荐用多线程的，他建议用多进程，变成一个个节点的形式进行通信。多线程分为两种模式：同步和异步。

• 同步：MultiThreadSpinner s(4)，一共5个线程。包括了主线程。

• 异步：AsyncSpinner s(4)， 一共5个线程。包括了主线程。

对于多话题的订阅，我们先看传统的方法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

void cb1(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    ROS_INFO("uwb_pose x: %f", msg->pose.position.x);
}

void cb2(const geometry_msgs::PoseStamped::ConstPtr& msg) 
{
    sleep(2);
    ROS_INFO("yolo_pose x: %f", msg->pose.position.x);
}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "node");
    ros::NodeHandle nh;
    setlocale(LC_ALL, "");

    ros::Subscriber uwbSub = nh.subscribe<geometry_msgs::PoseStamped>("uwb_pose", 1, cb1);
    ros::Subscriber yoloSub = nh.subscribe<geometry_msgs::PoseStamped>("yolo_pose", 1, cb2);
    ros::spin();
    return 1;
}

把代码加上ros::MultiThreadedSpinner s(2); ( 无需加入头文件 ), ros::spin();改为ros::spin(s);, 再运行会发现cb1里没有缺少一个消息。

这里多线程的目的是保证线程cb1不丢失消息，而不是cb2，它丢失消息是必然的。

对于ros::AsyncSpinner，代码在ros::Subscriber定义之后这样写:

1
2
3

ros::AsyncSpinner spinner(2);
spinner.start();
ros::waitForShutdown();

当程序当中有数据处理线程的时候，建议开辟异步多线程订阅，算法写在订阅函数里面。 当然，目前的处理当中，我更倾向于重新开辟一个线程，然后通过循环数组来进行数据交互。

参考：ROS多线程订阅消息