目录
一、概述
该包是从autoware中提取出来，能够实现基于激光雷达点云定位功能的最小包。
注意：使用该包完成定位功能前，需要先通过激光SLAM算法获取环境pcd点云地图。
该包的定位算法种类：
- 1.纯激光雷达点云定位
- 2.融合GNSS的点云定位
- 3.融合IMU的点云定位
- 4.融合GNSS、IMU的点云定位
二、功能包文件组织结构
该定位功能必须包含包：
autoware_build_flagsmessages
gnss_localizer的CMake依赖:
gnss
lidar_localizer的CMake依赖:
pcl_omp_registration-
autoware_health_checker->ros_observer -
points_downsampler->velodyne_pointcloud(apt安装) -
ndt_cpu->ndt_tku -
jsk_rviz_plugins(apt安装)
map_file的CMake依赖:
vector_map
编译使用catkin build，不能用catkin_make，会因编译包的先后顺序导致报错。
功能包文件组织结构
三、安装
3.1 安装依赖
sudo apt-get install python-catkin-tools ros-melodic-jsk-rviz-plugins ros-melodic-velodyne-pointcloud ros-melodic-nmea-msgs
3.2 编译
mkdir ~/catkin_localization/src
cd ~/catkin_localization/src
git clone xx
cd ..
catkin build
四、使用
4.1 快速开始
4.1.1 加载点云地图
roslaunch autoware_quickstart_examples small_robot_map.launch
4.1.2 开启NDT定位
roslaunch autoware_quickstart_examples small_robot_localization.launch
4.1.3 打开rviz，手动给初始位姿
注意初始位姿在(0, 0, 0)，如果给到了其它位姿，则会出现定位错误。
rviz -d default.rviz
4.1.4 播放数据集
因为是播放数据集，需要设置仿真时间
rosparam set use_sim_time true
播放数据集，默认暂停，按空格开始播放，
rosbag play --pause -k grass.bag /velodyne_points:=/points_raw
4.1.5 带GPS定位的节点关系图
如果有gps话题，则不需要在rviz中手动给初始位姿，记得在ndt的launch文件中启动gps定位选项。
其中fix为fix2tfpose节点订阅的话题名称，消息类型为sensor_msgs/NavSatFix
/points_raw为voxel_grid_filter节点订阅的话题名称，消息类型为sensor_msgs/PointCloud2
4.2 实车测试
4.2.1 通过SLAM算法建立环境点云地图
若定位要融合GPS，建图时需要知道建图原点处的GPS信息。
4.2.2 实时点云的frame_id与话题名称匹配
激光雷达点云的tf坐标系绑定在了/velodyne上，话题名称为/points_raw。
4.2.3 加载点云地图
roslaunch autoware_quickstart_examples small_robot_map.launch
注意不同的建图方法会导致pcd.width参数计算的不同，详情见5.1节中有序点云方式加载和无序点云方式加载的区别。
4.2.4 开启NDT定位
roslaunch autoware_quickstart_examples small_robot_localization.launch
4.2.5 打开rviz，手动给初始位姿
如果没有GPS，则该步骤是必须的！
4.2.6 实车节点关系图
从左往右看。
其中/rslidar_sdk_node是速腾16线程激光雷达的官方驱动节点，/rslidar_points是该驱动发布的话题，/points_raw是将速腾激光雷达点云的话题转换为velodyne激光雷达点云的话题，两者数据格式略有区别。未尝试不转换，可能不转换也可以。
/points_raw是经过voxel_grid_filter下采样后，发布/filtered_points，/ndt_maching节点订阅该话题，发布/ndt_pose，/ndt_pose是pub_odom节点订阅的话题，该话题给move_base提供里程计信息。
五、解析
5.1 map_file功能包
该功能包可加载3D地图点云和矢量地图，此处仅用点云地图。
source devel/setup.bash
roslaunch autoware_quickstart_examples small_robot_map.launch
robot_map.launch代码如下，通过指定点云地图文件路径加载点云地图，发布点云地图话题 /points_map。
<launch>
<rosparam command="load" file="$(find autoware_quickstart_examples)/config/headless_setup.yaml" />
<!-- TF -->
<include file="$(find autoware_quickstart_examples)/launch/small_robot_demo/small_robot_tf.launch"/>
<!-- Point Cloud -->
<node pkg="map_file" type="points_map_loader" name="points_map_loader" args="noupdate $(env HOME)/autoware_sync_dataset/c_test_place.pcd"/>
</launch>
其中small_robot_tf.launch文件发布静态tf变换，headless_setup.yaml是NDT定位中的tf参数配置
small_robot_tf.launch代码如下，设置静态发布的tf树
主要指定world->map；base_link->velodyne;base_link->mobility;这三个静态tf变换，具体参数可以根据自己的机器人进行调整。默认world与map重合，map与mobility重合。
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map" />
<node pkg="tf2_ros" type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_velodyne" args="0.12 0 -0.3 0 0 0 /base_link /velodyne" />
</launch>
headless_setup.yaml代码如下，指定NDT matching节点中激光雷达与车体坐标系之间的tf变换关系。
#T_baselink_velodyne
tf_x: 0.12 #0.12
tf_y: 0
tf_z: -0.3 #0.35
tf_yaw: 0
tf_pitch: 0
tf_roll: 0
localizer: velodyne
use_sim_time: false
注意!!!
如果rviz中无法显示/points_map地图点云，可能是通过slam算法建立pcd点云文件width参数不匹配。
1。以有序点云方式加载
修改map_file/nodes/points_map_loader/points_map_loader.cpp中342行
pcd.width = int(pcd.data.size()/32);
中的32改为16。
2。以无序点云方式加载
用cloudcompare等3D点云编辑工具打开pcd点云文件，查看软件终端显示的点云数量，将该值直接赋值，此时需要指定pcd.height=1，例如
pcd.width = 23738455;
pcd.height = 1;
可以修改源码，并且在启动节点时候添加output="screen"参数，将pcd.width参数打印出来。
<node pkg="map_file" type="points_map_loader" name="points_map_loader" args="noupdate $(env HOME)/autoware_sync_dataset/c_test_place.pcd"/> output="screen"
5.2 gnss_localizer与gnss功能包
gnss_localizer包主要实现将两种GPS消息类型（nmea_msgs/Sentence与sensor_msgs/NavSatFix）转换为UTM坐标，其中GPS坐标(WGS84)转UTM坐标算法具体实现是在gnss功能包中。
将nmea_msgs/Sentence转换为UTM坐标
roslaunch gnss_localizer nmea2tfpose.launch
将sensor_msgs/NavSatFix转换为UTM坐标
roslaunch gnss_localizer fix2tfpose.launch
其中nmea2tfpose.launch代码如下，基于第7个点的GPS坐标作为原点进行UTM转换计算。
<!-- -->
<launch>
<arg name="plane" default="7"/>
<node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
<param name="plane" value="$(arg plane)"/>
</node>
</launch>
若原点离实际位置太远，转换为UTM坐标误差较大。
原点参数:0-19，每个参数都对应一个GPS坐标点，在gnss/src/geo_pos_conv.cpp中设置，在此文件中把7设置为了北京的GPS坐标，。
void geo_pos_conv::set_plane(int num)
{
int lon_deg, lon_min, lat_deg, lat_min; // longitude and latitude of origin of each plane in Japan
if (num == 0)
{
lon_deg = 0;
lon_min = 0;
lat_deg = 0;
lat_min = 0;
}
else if (num == 1)
{
lon_deg = 33;
lon_min = 0;
lat_deg = 129;
lat_min = 30;
}
...
else if (num == 7) // beijing
{
lon_deg = 40; //36
lon_min = 0;
lat_deg = 116; //137
lat_min = 10;
}
...
}
若要重映射话题名称，可以在launch文件中设置remap参数
<!-- -->
<launch>
<arg name="plane" default="7"/>
<node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
<param name="plane" value="$(arg plane)"/>
<remap from="nmea_sentence" to="your_nmea_sentence_topic_name"/>
<remap from="gnss_pose" to="your_gnss_pose_topic_name"/>
</node>
</launch>
其中
-
nmea_sentence为nmea2tfpose节点订阅的话题名称，消息类型为nmea_msgs/Sentence -
gnss_pose为nmea2tfpose节点发布的话题名称，消息类型为geometry_msgs/PoseStamped。
fix2tfpose.launch代码如下，基于第7个点的GPS坐标作为原点进行UTM转换计算，也是调用gnss包进行UTM坐标计算。
<!-- -->
<launch>
<arg name="plane" default="7"/>
<node pkg="gnss_localizer" type="fix2tfpose" name="fix2tfpose" output="log">
<param name="plane" value="$(arg plane)"/>
</node>
</launch>
若要重映射话题名称，可以在launch文件中设置remap参数
<!-- -->
<launch>
<arg name="plane" default="7"/>
<node pkg="gnss_localizer" type="fix2tfpose" name="fix2tfpose" output="log">
<param name="plane" value="$(arg plane)"/>
<remap from="fix" to="your_fix_topic_name"/>
<remap from="gnss_pose" to="your_gnss_pose_topic_name"/>
</node>
</launch>
其中
-
fix为fix2tfpose节点订阅的话题名称，消息类型为sensor_msgs/NavSatFix -
gnss_pose为fix2tfpose节点发布的话题名称，消息类型为geometry_msgs/PoseStamped。
5.3 lidar_localizer包
lidar_localizer包主要实现基于NDT的点云定位算法，其中可以设置launch文件中的参数，选择是否融合GNSS、IMU、轮速里程计等进行融合定位。
roslaunch autoware_quickstart_examples small_robot_localization.launch
其中small_robot_localization.launch代码如下，主要是调用points_downsampler的launch文件和lidar_localizer的laucnh文件实现基于纯点云定位功能，可选融合GPS进行定位。
<launch>
<!-- setting path parameter -->
<arg name="get_height" value="true" />
<!-- points downsampler -->
<include file="$(find points_downsampler)/launch/points_downsample.launch" />
<!-- fix2tfpose -->
<!-- <include file="$(find gnss_localizer)/launch/fix2tfpose.launch"/> -->
<!-- ndt_matching -->
<include file="$(find lidar_localizer)/launch/ndt_matching.launch">
<arg name="get_height" value="$(arg get_height)" />
</include>
</launch>
其中get_height参数是用来指定ndt_matching是否获取高度信息，若为true，则获取高度信息，否则不获取高度信息。若假设机器人或无人车认为是在平面上运动，可以设置该参数为false，或者不设置该参数，在ndt_matching.launch文件中该参数默认为false。
points_downsample.launch代码如下，对实时激光雷达点云数据进行下采样。
<launch>
<arg name="sync" default="false" />
<arg name="node_name" default="voxel_grid_filter" />
<arg name="points_topic" default="points_raw" />
<arg name="output_log" default="false" />
<arg name="measurement_range" default="200" />
<node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)">
<param name="points_topic" value="$(arg points_topic)" />
<remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
<param name="output_log" value="$(arg output_log)" />
<param name="measurement_range" value="$(arg measurement_range)" />
</node>
</launch>
该节点订阅话题名称为points_raw，消息类型为sensor_msgs/PointCloud2。
若需要融合GPS数据，可以在small_robot_localization.launch文件中取消注释fix2tfpose节点。
ndt_matching.launch代码如下，其中对ndt所需要的参数进行了配置，主要的参数有：1 默认使用cpu；2 默认不使用gnss；3 默认不使用odom；4 默认不使用imu；5 默认不使用IMU方向正负变换等
<launch>
<arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
<arg name="use_gnss" default="false" />
<arg name="use_odom" default="false" />
<arg name="use_imu" default="false" />
<arg name="imu_upside_down" default="false" />
<arg name="imu_topic" default="/imu_raw" />
<arg name="queue_size" default="1" />
<arg name="offset" default="linear" />
<arg name="get_height" default="false" />
<arg name="use_local_transform" default="false" />
<arg name="sync" default="false" />
<arg name="output_log_data" default="false" />
<arg name="gnss_reinit_fitness" default="500.0" />
<node pkg="lidar_localizer" type="ndt_matching" name="ndt_matching" output="log">
<param name="method_type" value="$(arg method_type)" />
<param name="use_gnss" value="$(arg use_gnss)" />
<param name="use_odom" value="$(arg use_odom)" />
<param name="use_imu" value="$(arg use_imu)" />
<param name="imu_upside_down" value="$(arg imu_upside_down)" />
<param name="imu_topic" value="$(arg imu_topic)" />
<param name="queue_size" value="$(arg queue_size)" />
<param name="offset" value="$(arg offset)" />
<param name="get_height" value="$(arg get_height)" />
<param name="use_local_transform" value="$(arg use_local_transform)" />
<param name="output_log_data" value="$(arg output_log_data)" />
<param name="gnss_reinit_fitness" value="$(arg gnss_reinit_fitness)" />
<remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
</node>
</launch>
注意！！！
在基于ndt_matching节点进行定位时，需要给定/initialpose。若融合了GNSS，则由GNSS提供初始位姿，否则需要手动给/initialpose初始位姿，不然ndt_matching节点无法完成初始化。
手动给/initialpose初始位姿的方法：通过rviz的工具栏2D Pose Estimate给定初始位姿。注意该位姿是在world坐标系下的，所以手动给定初始位姿时，最好打开官方的rviz配置文件，然后再给，
rviz -d default.rviz。
TODO:
1。确定好用哪个数据集。解决grss还有高度的问题。高度问题不包含z即可。
