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;

该路径规划器的主要思想，就是通过点云数据寻找障碍物，然后剔除被障碍物遮挡的路径线条。保留可通行的路径，在所有可通行的路径中，根据评分公式，选择一条最可能接近目标点的路径。

问题

导航过程中，路径不断切换，追踪代码发现selectedGroupID出现大量-1，在-1和正数之间不断切换。 -1其实就是没找到路径。 再向上查，发现之所以成为-1，是maxScore和clearPathPerGroupScore[i]比较时，后者一直为0， maxScore不更新。 继续查，clearPathPerGroupScore的赋值是在

1
2

float score = (1 - sqrt(sqrt(dirWeight * dirDiff))) * rotDirW * 
               rotDirW * rotDirW * rotDirW * penaltyScore;

1
2

dirDiff: 114.517456, rotDirW: 9.000000, penaltyScore: 1.000000
pathPenaltyList[i]: 0.000000, costHeightThre: 0.100000

不重要的是ps3.launch， rviz， 这里只列出重要部分

loam_interface.launch

1
2
3
4
5
6
7
8
9
10
11
12

<launch>

  <node pkg="loam_interface" type="loamInterface" name="loamInterface" output="screen" required="true">
    <param name="stateEstimationTopic" type="string" value="/integrated_to_init" />
    <param name="registeredScanTopic" type="string" value="/velodyne_cloud_registered" />
    <param name="flipStateEstimation" type="bool" value="true" />
    <param name="flipRegisteredScan" type="bool" value="true" />
    <param name="sendTF" type="bool" value="true" />
    <param name="reverseTF" type="bool" value="false" />
  </node>

</launch>

• 参数stateEstimationTopic换成世界坐标系下的机器人当前位姿，实际要求的是里程计消息，我的是从LOAM发布的final_odom

• 参数registeredScanTopic为LOAM框架输出的关键帧话题，我的是Lego-LOAM的current_scan

• 参数flipStateEstimation 和 flipRegisteredScan改为false，只要雷达位置没有旋转。否则启动后，机器人坐标系是旋转过的

loam_interface的代码太简单了： 不断接收SLAM部分提供的世界坐标系下的机器人当前位姿 以及 世界坐标系下的当前关键帧，并分别以state-estimastion（换个名字和发布TF） 和 registered_scan(换个名字)话题名发布出去

sensor_scan_generation.launch

订阅/state_estimation和 /registered_scan，执行 laserCloudAndOdometryHandler，并发布state_estimation_at_scan 和 sensor_scan

订阅state_estimation以及registered_scan，执行回调函数。

发布/state_estimation_at_scan（等同于/state_estimation）

发布/sensor_scan（世界坐标系下的点云转换至传感器Lidar坐标系下）