机器人导航时偶然出现倒退的小速度-0.02，但周围又没有障碍物，其实就是参数max_vel_x_backwards=0.02

定义矩阵

a=[1,2,3; 4,5,6]是一个2×3的矩阵

1
2

1 2 3
4 5 6

matrix = [1;2;3;4;5] 是5x1的矩阵， matrix = [1,2,3,4,5]是1x5的矩阵

matrix = [1,2,3,4,5]' 是做了转置，结果是5x1的矩阵

matrix(:, 1)是矩阵的第一列

• zeros(m,n): 生成一个 m 行 n 列的零矩阵，m=n 时可简写为 zeros(n)

• ones(m,n): 生成一个 m 行 n 列的元素全为 1 的矩阵， 当m=n 时可写为 ones(n)

• eye(m,n): 生成一个主对角线全为 1 的 m 行 n 列矩阵， m=n 时可简写为 eye(n)，即为 n 维单位矩阵

• rand(m,n): 产生 0～1 间均匀分布的随机矩阵， m=n 时简写为 rand(n)

• randn(m,n): 产生均值为0，方差为1的标准正态分布矩阵，m=n 时简写为 randn(n)

zeros(m, n)中的n可以为0，此时矩阵实际是空。

1
2
3

matrix = zeros(5,0)
matrix = [matrix, [1;2;3;4;5] ]
matrix = [matrix, [11;12;13;14;15] ]

1
2
3
4
5

1    11
2    12
3    13
4    14
5    15

这篇论文发表于2021.10，大部分内容在官网上，对一些重点内容做一下记录。

I. INTRODUCTION

算法框架包括探索算法TARE (exploration planner)； 路径算法FAR (route planner)

我们的算法不依赖只有仿真才提供的信息，比如 semantic and terrain segmentation groundtruth. 算法是为了让用户重点开发上层的规划算法。

在2021 DARPA地下挑战赛中， CMU-OSU Team Explorer 虽然没拿到冠军，但获得”探索最多区域奖“(26 out of 28 sectors).

II. RELATED WORK

For outdoor settings, the most well-known dataset is KITTI Vision Benchmark Dataset [4] collected
from a self-driving suite in road driving scenarios. The
dataset contains sensor data from stereo cameras, 3D Lidar,
and GPS/INS ground truth for benchmarking depth reconstruction, odometry estimation, object segmentation, etc.

For localization purposes, the long-term localization benchmark includes a comprehensive list of datasets: Aachen Day-Night dataset, Extended CMU Seasons dataset, RobotCar Seasons dataset, and SILDa Weather and Time of Day dataset.

For indoor navigation, iGibson [15], [16], Sapien [17], AI2Thor [18], Virtual Home [19], ThreeDWorld [20], MINOS [21], House3D [22] and CHALET [23] use synthetic scenes, while reconstructed scenes are also available in iGibson, AI Habitatand MINOS. Compared to datasets, simulation environments have the advantage of providing access to ground truth data, e.g. vehicle pose and semantic segmentation to simplify the algorithm development and allowing full navigation system tests in closed control loop。

For indoor scenes, datasets such as NYUDepth dataset [7], TUM SLAM dataset [8], inLoc dataset [9], MIT Stata center dataset, and KTH-INDOL dataset [11], [12] are available. Datasets are useful in developing and

Simulation environments: Carla [13] and AirSim [14] are two representative simulation environments for autonomous driving and flying. These simulators support various conditions such as lighting and weather changes, moving objects such as pedestrians, and incident scenes.

其他的导航框架: ROS Navigation Stack 和港科大的 fast_planner ，不过是针对无人机的。

我们的导航框架: 支持photorealistic house models from Matterport3D 以及向AI Habitat提供接口。这两个在机器人学和计算视觉中应用很多。Users are provided with scan data and RGB, depth, and semantic images rendered by AI Habitat. Users have the option of running AI Habitat side by side with our system or in post-processing

参考：
阅读笔记 《Matterport3D: Learning from RGB-D Data inIndoor Environments》
AI Habitat室内仿真平台使用

III. DEVELOPMENT ENVIRONMENT

Local Planner

local planner保证了到达目标时的安全，它预计算 a motion primitive library and associates the motion primitives to 车周围的3D locations(这里说的就是free_paths对应的曲线簇)。 The motion primitives are modeled as Monte Carlo samples and organized in groups

现实中快遇到障碍物时，local planner可以在几毫秒内判断出哪些 motion primitives 会和障碍相撞，然后从motion primitives中选出最可能朝向目标的一组。

The module also has interface to take in additional range data for collision avoidance as an extension option

Terrain Traversability Analysis

模块建立了一个代价地图，地图中的每一个点和一个traversal cost相关联. 代价值取决于 local smoothness of the terrain.

我们使用体素网格表示环境，分析 distributions of data points in adjacent voxels to 估计地面高度. The points are associated with higher traversal costs if they are further apart from the ground.

模块还能处理negative obstacles，这些障碍生成空区域with no data points on the terrain map. 如果开启了negative obstacle的处理，模块认为那些区域不可经过

Visualization and Debugging Tools

可视化算法的性能. 可视化工具显示地图，已探索区域，车的轨迹. Metrics包括 explored volume, traveling distance, and algorithm runtime are plotted and logged to files.

另外，支持手柄和导航交互，在多种操作模式之间切换 to ease the process of system debugging.

IV. HIGH-LEVEL PLANNERS

V. BEST PRACTICES

Safety margin: local planner使用车和waypoint之间的距离作为规划尺度(planning horizon)。即使waypoint很接近障碍，也可以让车到达。但是，如果车不能在waypoint停下，上层规划器倾向于让车到waypoint保持一定距离(默认≥
3.75m)。If the waypoint is closer, users can project the waypoint further away and keep the waypoint in the same direction w.r.t. the vehicle to fully use the safety margin. (这句话没理解什么意思)

但是，如果车需要经过窄通道，降低这一距离可以让local planner找到安全合适的路径。

大转弯: Typically, a high-level planner selects the waypoint along the path that is a distance, namely look-ahead distance, ahead of the vehicle and sends the waypoint to the local planner (possibly after projecting the waypoint further away from the vehicle as discussed above).

When handling sharp turns (≥ 90 deg), the look-ahead distance needs to be properly set or the waypoint may jump to the back of the vehicle, causing the vehicle to osculate back-and-forth. We recommend to select the waypoint on the starting segment of the path that is in line-of-sight from the vehicle.

动态障碍: terrain analysis模块在动态障碍走过之后，通过ray-tracing清除动态障碍。这是在车的附近范围实现的(到车的距离≤5m)， 因为雷达数据在远处比较稀疏，以及难以权衡清除动态障碍和thin structures之间的矛盾。 用户根据自己的情况来清除动态障碍，TARE和FAR都有这一步骤。

VI. SYSTEM INTEGRATION

用于DARPA挑战赛的平台

The state estimation module can detect and introduce loop closures. The module outputs state estimation in the odometry frame generated by 3D Lidar-based odometry containing accumulated drift. When loop closure is triggered, it outputs loop closure adjustments to globally relax the vehicle trajectory and corresponding maps. Loop closure adjustments are used by the high-level planners since they are in charge of planning at the global scale.

The local planner and terrain analysis modules are extended to handle complex terrains including negative obstacles such as cliffs, pits, and water puddles with a downward-looking depth sensor. The TARE planner, FAR planner, and other planners (for stuck recovery, etc) are run in parallel for tasks such as exploration, go-to waypoint, and return home.

On top of these planners, behavior executive and multi-robot coordination modules are built specifically for the challenge. The modules share explored and unexplored areas across multirobots and call TARE and FAR planners cooperatively. In particular, when a long-distance transition is determined due to new areas containing artifacts are discovered or operator waypoints are received, the behavior executive switches to
FAR planner for relocating the vehicle. During the relocation, FAR planner uses a sparse roadmap merged from multi-robots for high-level guidance. After the relocation is complete, TARE planner takes place for exploration

DARPA最后决赛是在Louisville Mega Cavern, KY. 赛道包括隧道、urban、洞穴环境。我们的三台车使用TARE 和 FAR进行探索。

红色轨迹：车先使用TARE一直探索到B，然后使用FAR经过C，一到达C，车换成TARE进行剩下的探索。

绿色轨迹：车只用TARE；蓝色轨迹：车先使用FAR经过A，在换成TARE进行之后的搜索。

三台车各行走596.6m, 499.8m, 445.2m。共花费时间2259s

这里只看论文不太明白，看实际视频才清楚。实际是两辆车和一架无人机进行探索。

VII. EXTENDED APPLICATION EXAMPLES

这部分我暂时用不到，就不研究了。

训练基于学习的探索和导航算法。大家可以使用我们的仿真平台作为专家系统(expert system)来训练基于学习的规划算法。专家系统里包含状态估计，局部避障，上层的探索或者路径规划算法。训练的对象可以是一些端对端的方法，比如只有一层深度网络的探索或者路径规划算法。

基于激光或者视觉的sim-to-real学习。大家可以用我们的仿真平台训练一些基于学习的方法，并将训练好的系统应用在现实世界中。因为我们提供了多个仿真世界以及支持来自Matterport3D的90个photorealistic环境模型，并且兼容AI Habitat的RGB，深度，和语义(Semantic)图像的渲染功能，所以不管是基于激光还是视觉的算法，大家都可以找到适用的环境模型，当然也可以根据需求换上自己的模型。举个简单的例子，需要做Semantic-in-loop导航的人可以使用我们提供的带有语义信息的环境，使用我们的地面机器人仿真平台，去训练他们的算法，并且在训练结束后，直接将算法移植到实际的机器人上。下面是我们的仿真机器人在Matterport3D的两个环境模型中导航的场景，用AI Habitat实时生成RGB，深度及语义图像。

开发人群导航(social navigation)算法。大家可以在我们的仿真环境中加入动态的障碍物或者行人，模拟真实环境中的社区场景，辅助开发人群导航算法，甚至可以在这种场景中预测动态物体的行为及其轨迹。

Dual-Stage Viewpoint Planner 包含两个阶段：探索阶段用于扩展地图边界，重定位阶段用于 explicitly 把机器人传到环境中不同的子区域。探索阶段采用RRT，并动态地扩展RRT over replanning steps. 重定位阶段维护一个graph结构 through the mapped environment. 在探索过程中，算法在两个阶段来回切换，以探索整个环境。

dsvp_launch 的使用

1
2
3
4

sudo apt update
sudo apt install ros-melodic-octomap-ros libgoogle-glog-dev libgflags-dev
# 取melodic分支
git clone https://github.com/HongbiaoZ/dsv_planner.git

编译结束后，运行

1
2
3

# 车库仿真环境，可以换成其他的
roslaunch vehicle_simulator system_garage.launch
roslaunch dsvp_launch explore_garage.launch

然后是Matterport3D环境的配置，先跳过：To run DSV Planner in a Matterport3D environment, follow instructions to setup the development environment to use the Matterport3D environment. Then, use the command lines below to launch the system and DSV Planner.

1
2

roslaunch vehicle_simulator system_matterport.launch
roslaunch dsvp_launch explore_matterport.launch

local_planner.launch包括节点localPlanner和pathFollower，以及发布sensor ---> vehicle 和 sensor ---> camera的tf变换

• 避障: 由local_planner实现. 程序一启动就先预生成 Motion primitives. The motion primitives are modeled as Monte Carlo samples and organized in groups 现实中快遇到障碍物时，local planner可以在几毫秒内判断出哪些 motion primitives 会和障碍相撞，然后从motion primitives中选出最可能朝向目标的一组。

• Sending waypoints, navigation boundary, and speed: 一收到waypoint, navigation boundary, and speed 的消息, the system will navigate the vehicle inside the navigation boundary to the waypoint. 发送的navigation boundary和速度是可设置的，默认速度2m/s. 可以参考waypoint_example如何发送这些消息

• Waypoint: 话题way_point，geometry_msgs::PointStamped类型的消息，在map坐标系。 发布者当然是rvizGA 和 localPlanner

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

  header: seq: 6 stamp: secs: 215 nsecs: 380000000 frame_id: "map" point: x: 9.90468215942 y: 12.2474937439 z: 0.839443683624 --- header: seq: 7 stamp: secs: 215 nsecs: 380000000 frame_id: "map" point: x: 9.90468215942 y: 12.2474937439 z: 0.839443683624

• Navigation boundary (optional): 话题navigation_boundary，geometry_msgs::PolygonStamped的消息， 在map坐标系。 默认没有发布，订阅者是localPlanner

system_real_robot.launch中的部分：

1
2
3
4
5

<include file="$(find local_planner)/launch/local_planner.launch" >
  <arg name="cameraOffsetZ" value="$(arg cameraOffsetZ)"/>
  <arg name="goalX" value="$(arg vehicleX)"/>
  <arg name="goalY" value="$(arg vehicleY)"/>
</include>

这个launch其实就是localPlanner 和 pathFollower两个节点，再加两个tf转换

1
2
3

<node pkg="tf" type="static_transform_publisher" name="vehicleTransPublisher" args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/>

<node pkg="tf" type="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(arg cameraOffsetZ) -1.5707963 0 -1.5707963 /sensor /camera 1000"/>

节点localPlanner一直在发布话题path，可以将源码中的宏PLOTPATHSET改为0，最好是做成ROS Param.

只有path follower会发布cmd_vel，在机器人前往目标点的过程中， local planner会不断的对比机器人当前位置和目标点位置的差距，根据这个差值去控制机器人的直线速度和转向速度，最终不断逼近目标点。

代码中常出现的laserCloud和vehicleX都是在map坐标系下

Matlab文件path_generator.m的目的是生成3个ply文件和correspondences.txt。这里就是一个可优化的方向。

生成的的是3个离线的曲线，也就是local_planner\paths的startPaths.ply, pathList.ply, paths.ply。 这3个文件都在localPlanner.cpp的main函数里使用

startPaths.ply文件的文件头

1
2
3
4
5
6
7
8

ply
format ascii 1.0
element vertex 707  # 点的总数
property float x
property float y
property float z
property int group_id
end_header

同理还有paths.ply

记录三次采样生成的所有路径点，仔细数一数，共有7x7x7=343条路径(第二次采样的路径组分成的7个不应统计)， 这和论文里说的35x35=1225不同了。 每条路径有301个点，最右边的点，也就是x最大的点是(3, 0)。最上方的点大约(1.679, 2.486)
代码中对应函数readPaths: 读取文件paths.ply，把其中的点都放入点云类型的变量paths，但考虑 pointSkipNum

后两段段还是三次样条曲线，但生成的方式有所改变，不再是简单的1:0.01:2的遍历，而是对角度做了一定比例的缩放，并不是偏移-54°，而是偏移-44.55°和-55.96°。公式比较复杂，就不深入研究了

以最下方的曲线为例，三段的最后一个点的坐标如下：

该文件记录了每条路径的最后一个路径点，共7x7x7=343个点
代码中对应函数readPathList: 读取文件pathList.ply，只加入变量pathList 和 endDirPathList

体素网格的生成

path_generator.m中的部分如下

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

# 对于碰撞检查，使用覆盖了传感器范围的体素网格。
# 根据样条距离，传感器范围为 3.2*4.5 ,在该区域生成体素网格，在这里考虑了车辆半径的遮挡

voxelSize = 0.02;
searchRadius = 0.45;
# 根据pathList的形状确定的尺寸，比模拟路径的范围稍大点
offsetX = 3.2;
offsetY = 4.5;
# offsetX / voxelSize
voxelNumX = 161;
# 这里是 offsetY * 2 / voxelSize, 体素范围关于x轴对称
voxelNumY = 451;

fprintf('\nPreparing voxels\n');

indPoint = 1;
voxelPointNum = voxelNumX * voxelNumY;
 # 一个有很多行 只有2列的矩阵
voxelPoints = zeros(voxelPointNum, 2);

# 在外层循环中，从外向内计算，同采样的路径一样，靠近车体的位置Y的宽度也会随着scaleY减小
for indX = 0 : voxelNumX - 1
    x = offsetX - voxelSize * indX;
    scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX;
    for indY = 0 : voxelNumY - 1
        y = scaleY * (offsetY - voxelSize * indY);

        voxelPoints(indPoint, 1) = x;
        voxelPoints(indPoint, 2) = y;
        
        indPoint  = indPoint + 1;
    end
end

# 所有坐标都存到了 voxelPoints, zeros(voxelPointNum, 1)相当于扩展了一列 0
plot3(voxelPoints(:, 1), voxelPoints(:, 2), zeros(voxelPointNum, 1), 'k.');
pause(1.0);

matlab运行结果得到 fig图 如下，如果放大，会发现是密密麻麻的点

再放大，会发现前向的模拟路径在里面

之所以把体素建模为梯形而不是三角形，应该是在起始点考虑了车辆的宽度。

correspondences表

correspondences.txt在代码中的readCorrespondences函数使用

path_generator.m中的代码如下：

1
2
3
4
5
6

fprintf('\nCollision checking\n');
# rangesearch（X,Y,r）：寻找X集合中的点，其与 Y 集合中的点的距离小于r
# 这里就是寻找 paths 的点 和 体素集合中的点中小于搜索半径的点集合
# 记录路径点附近的所有体素网格
[ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius);
    # ...... 剩余的省略

最右边的一堆体素网格没有任何路径，这就是txt文件中只有网格索引和-1的情况。

1
2
3
4
5
6
7
8
9

sudo apt update
sudo apt install -y python-catkin-pkg python-rosdep ros-$ROS_DISTRO-catkin
sudo apt install -y python3-pip python3-colcon-common-extensions python3-setuptools python3-vcstool
pip3 install -U setuptools

# 安装 vcs
sudo apt-get install python3-vcstool
# 安装 colcon
sudo apt install python3-colcon-common-extensions

创建工程目录mkdir -p autoware.ai/src，把代码下载到这里，我已经提前下载好，把autoware.ai.repos也放进去

1
2
3
4
5
6
7
8

# 安装依赖项，时间很久
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

# 若没有 CUDA支持，只使用 CPU 编译，完全编译的时间更久
# colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release

# 只编译一个包 autoware_camera_lidar_calibrator
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select autoware_camera_lidar_calibrator image_processor calibration_publisher

同理可以只编译runtime_manager，启动在src/utilities/runtime_manager/launch/runtime_manager.launch

1. 从OpenCV的github release下载，然后解压

   1 2 3 4 5 6

   cd opencv-4.2.0 mkdir build cd build cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. make sudo make install

   添加路径库 sudo vim /etc/ld.so.conf.d/opencv.conf，打开了一个新文档，在里面写入/usr/local/lib

2. 配置环境变量sudo vim /etc/profile，在后面添加以下内容

   1 2	PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig export PKG_CONFIG_PATH

3.测试

1
2
3
4
5
6

cd ~
# opencv的源码目录
cd opencv/samples/cpp/example_cmake
cmake .
make
./opencv_example

1. 检查是否有多个OpenCV版本

   1 2 3 4 5 6 7 8 9

   locate libopencv_video.so /usr/lib/x86_64-linux-gnu/libopencv_video.so /usr/lib/x86_64-linux-gnu/libopencv_video.so.3.2 /usr/lib/x86_64-linux-gnu/libopencv_video.so.3.2.0 /usr/local/lib/libopencv_video.so /usr/local/lib/libopencv_video.so.4.2 /usr/local/lib/libopencv_video.so.4.2.0

1. 如果你需要在Python3环境下使用OpenCV，那么sudo pip3 install opencv-python，python后不用加3。对于在Python环境中使用,比如说查看版本

   1 2 3 4 5 6 7

   cyp@cyp:~$ python Python 3.6.7 (default, Oct 22 2018, 11:32:17) [GCC 8.2.0] on linux Type "help", "copyright", "credits" or "license" for more information. >>> import cv2 as cv >>> cv.__version__ '4.1.0'

在使用g++编译使用opencv的C++程序时，使用 g++ <cpp_code> pkg-config opencv --libs --cflags opencv 也可以使用cmake编译

1. 使用指定的版本

在opencv编译好后，所在目录中一般会有一个叫OpenCVConfig.cmake的文件，这个文件中指定了CMake要去哪里找OpenCV，其.h文件在哪里等，比如其中一行：

1
2

# Provide the include directories to the caller 
set(OpenCV_INCLUDE_DIRS "/home/ubuntu/src/opencv-3.1.0/build" "/home/ubuntu/src/opencv-3.1.0/include" "/home/ubuntu/src/opencv-3.1.0/include/opencv") 

1

set(OpenCV_DIR "/home/ubuntu/src/opencv-3.1.0/build")

话题

joy话题类型是sensor_msgs/Joy，发布者rvizGA，订阅者有:

• pathFollower
• terrainAnalysisExt
• localPlanner
• terrainAnalysis

Rviz上发布waypoint后，得到的joy消息，而且每次是相同的：

1
2
3
4
5
6
7
8

header:
  seq: 0
  stamp:
    secs: 1640595487
    nsecs: 707291876
  frame_id: "waypoint_tool"
axes: [0.0, 0.0, -1.0, 0.0, 1.0, 1.0, 0.0, 0.0]
buttons: [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]

查看joystickHandler函数，发现joySpeed 和 joySpeedRaw 都会被赋值为 1

话题speed也没有发布者，提供给用户，类型std_msgs::Float32，订阅者是pathFollower和localPlanner，目前看不出使用的必要。

话题stop提供给用户，用于停止车的行走，类型std_msgs::Int8

vehicleSpeed逐步增加，也就是说输出速度是逐渐增加的

参数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51

string pathFolder;     //  使用matlab生成路径集合的文件路径
double vehicleLength = 0.9;        //  车辆的长度，单位m
double vehicleWidth = 0.6;         //  车辆的宽度，单位m
double sensorOffsetX = 0;          //  传感器坐标系与车体中心的偏移量
double sensorOffsetY = 0;          //  传感器坐标系与车体中心的偏移量
bool twoWayDrive = true;           //  双向驱动
double laserVoxelSize = 0.05;      //  降采样体素栅格叶大小
double terrainVoxelSize = 0.2;     //  降采样体素栅格叶大小
bool useTerrainAnalysis = false;   //  是否使用地面分割后的点云信息
bool checkObstacle = true;          
bool checkRotObstacle = false;
double adjacentRange = 3.5;        //  裁剪点云时的距离
double obstacleHeightThre = 0.2;   //  障碍物高度阈值, 调整以account for terrain cloud thickness
double groundHeightThre = 0.1;     //  地面高度阈值
double costHeightThre = 0.1;       //  计算路径惩罚得分的权重
double costScore = 0.02;           //  最小惩罚得分
bool useCost = false;   
const int laserCloudStackNum = 1;   // 缓存的激光点云帧数量
int laserCloudCount = 0;            // 当laserCloudStackNum = 1时,暂时没用到
int pointPerPathThre = 2;     // 每条路径需要有几个被遮挡的点
double minRelZ = - 0.5;        // 未使用地面分割时，裁剪点云时的最小高度
double maxRelZ = 0.25;      // 未使用地面分割时，裁剪点云时的最大高度
double maxSpeed = 1.0;      // 最大速度
double dirWeight = 0.02;    // 计算得分时转向角度的权重
double dirThre = 90.0;              // 最大转向角度
bool dirToVehicle = false;          // 是否以车辆为主方向计算被遮挡的路径
double pathScale = 1.0;      // 路径尺度
double minPathScale = 0.75;         // 最小路径尺度
double pathScaleStep = 0.25;        // 路径尺度的调整步长
bool pathScaleBySpeed = true;       // 是否根据速度调整路径尺度
double minPathRange = 1.0;          // 最小路径距离
double pathRangeStep = 0.5;         // 路径范围的调整步长
bool pathRangeBySpeed = true;       // 是否根据速度调整路径的范围
bool pathCropByGoal = true;         // 是否根据目标点+ goalClearRange 筛选点云数据
bool autonomyMode = false;
double autonomySpeed = 1.0;
double goalClearRange = 0.5;        // 当 pathCropByGoal = true 时,点云距离超过目标点+该值则不被处理
double goalX = 0;        // 局部路径目标点
double goalY = 0;        // 局部路径目标点


// 离线路径的参数
const int pathNum = 343;
const int groupNum = 7;
float gridVoxelSize = 0.02;
float searchRadius = 0.45;
float gridVoxelOffsetX = 3.2;
float gridVoxelOffsetY = 4.5;
const int gridVoxelNumX = 161;
const int gridVoxelNumY = 451;
const int gridVoxelNum = gridVoxelNumX * gridVoxelNumY;