autoware部署一：基于sc-lego-loam建图与ndt-mathing定位

Lego-loam是一种轻量级的建图算法，能够在一些小型载板上使用，如NVIDIA出厂的NX，能够进行三维的建图。比autoware自带的ndt_mapping精度更好，建图效率更好，能够适应大型的场景。所以就想将autoware自带的建图算法替换成lego-loam，但是lego-loam与ndt-matching使用的坐标系不一致，导致建图完的pcd输入与实际场景是不一致的，地图是倒过来的，因此。需要变换一下建图的点云输出，因此，找到了SC-lego-loam算法实现这一转换。

1、sc-lego-loam建图

1.1 下载源码包

mkdir -p sc_lego_loam/src
cd sc_lego_loam/src
git clone https://github.com/AbangLZU/SC-LeGO-LOAM.git
cd …
catkin_make

1.2.适配激光雷达驱动

原作者使用的是Ouster OS-64雷达，我使用的是镭神智能的c32激光雷达，需要更改utility.h文件适配。
首先修改pointCloudTopic：
原来：

//extern const string pointCloudTopic = “/os1_points”;

修改：

extern const string pointCloudTopic = “/velodyne_points”;

修改激光雷达部分参数：
原来：

// Ouster OS1-64
//extern const int N_SCAN = 64;
//extern const int Horizon_SCAN = 1024;
//extern const float ang_res_x = 360.0/float(Horizon_SCAN);
//extern const float ang_res_y = 33.2/float(N_SCAN-1);
//extern const float ang_bottom = 16.6+0.1;
//extern const int groundScanInd = 15;

修改：

extern const int N_SCAN = 32;
extern const int Horizon_SCAN = 2000;
extern const float ang_res_x = 360.0/float(Horizon_SCAN);
extern const float ang_res_y = 31/float(N_SCAN-1);
extern const float ang_bottom = 16.5;
extern const int groundScanInd = 15;

其他配置参考： http://blog.csdn.net/weixin_41281151/article/details/113558183

1.3 重新编译一下

cd sc_lego_loam/
catkin_make
source devel/setup.bash

1.4 适配镭神激光雷达驱动

镭神激光雷达发布的点云原始数据话题一般为lslidar_point_cloud，而lego_loam接收的点云话题为/velodyne_raw，所以需要将二者映射关系修改下，激光驱动launch文件加入：

<remap from="lslidar_point_cloud" to="/velodyne_points" />

Autoware接收的话题名为/points_raw，因此remap

<remap from="lslidar_point_cloud" to="/points_raw" />

镭神激光雷达点云使用的坐标系为laser_link，lego-loam使用的坐标系名字为velodyne，因此修改二者的不同

<!--param name="frame_id" value="laser_link"/-->
<param name="frame_id" value="velodyne"/>

1.5 建图

roslaunch lego-loam run.launch
rosbag play --clock ***.bag

sc_lego_loam算法已经添加了自动保存地图的功能，bag包播放结束后，使用ctrl+c即可结束，地图保存在map文件夹下。修改自动保存的pcd文件名为lab_indoor.pcd

2、使用ndt-matching进行点云匹配定位

2.1 启动autoware和开启gazebo仿真模拟

roslaunch runtime_manager runtime_manager.launch
roslaunch vehicle_gazebo_simulation_launcher world_test.launch

注：记得要先开autoware再开gazebo，主要是为了统一时间戳，否则时间戳会出错，导致tf变换报错，无法进行后续工作。

2.2 点云定位tf树介绍

world坐标系到map坐标系是由Map选项里的TF发布的，base_link到velodyne坐标系是由gazebo仿真发布的，map到base_link坐标系则是由ndt-matching发布的。

2.3 base_link 到 velodyne坐标变换

选择setup页面，驱动Localizer选择的是velodyne，设置tf，tf是激光雷达相对于车体中心的坐标变换。这一步其实在打开gazebo的时候就已经自动生成了，其实可以不需要。但是如果是跑simulation的话这一步是需要的。

2.4 加载地图+world到map坐标系变换

切换到Map页面，point cloud是加载建图生成的pcd文件，tf则是做world到map的坐标变换，vector map是加载矢量地图。后续会用到。

2.5 进行原始点云的滤波处理

Sensing最终配置如下图：

Voxel_grid_filter是体素滤波器，将一定范围内的点近似的看成是一个点，减少数据量，效率更高。Voxel Leaf Size参数设置为0.5，代表0.5米立方体内的电云都用一个重心点近似代替。Measurement Range参数为200，表示点云的有效距离为200米。

Ring ground filter作用是进行地面点的滤波，得到去除地面点的数据，发布的话题为/points_no_ground。Sensor Height为激光雷达距离地面的安装高度，Max Slope表示小于10°的坡会被认为是地面点，也会被滤除。Vertical Thres表示障碍物和地面的差异度，如果大于此值则被认为是障碍物。

2.6 启动ndt-matching定位

切换到computing页面，最终配置如下图：

点击lidar_localizer下ndt-matching进行配置，Initial Pos表示激光雷达初始位姿与建图时的初始位姿变换，如果有gnss数据可以使用gnss进行初始定位。其他默认。

启动vel_pose_connect，确定为勾选Simulation Mode

2.7 进行车辆的控制

启动键盘控制节点

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

控制车辆运动，可以看到rviz中激光雷达的原始数据与地图中的点云进行很好的匹配定位。

2.8 真实机器人控制

真实机器人的定位与建图就是把打开gazebo仿真的步骤换成打开lidar驱动程序和底盘驱动程序