标签：定位 map launch Autoware tf 建图 points 点云

转载自https://www.jianshu.com/p/38221263c4ee

一、概述

该包是从autoware中提取出来，能够实现基于激光雷达点云定位功能的最小包。

注意：使用该包完成定位功能前，需要先通过激光SLAM算法获取环境pcd点云地图。

该包的定位算法种类：

1.纯激光雷达点云定位
2.融合GNSS的点云定位
3.融合IMU的点云定位
4.融合GNSS、IMU的点云定位

二、功能包文件组织结构

该定位功能必须包含包：

autoware_build_flags
messages

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。

其中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重合。

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参数打印出来。

 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>

标签：定位,map,launch,Autoware,tf,建图,points,点云
From： https://www.cnblogs.com/radiumlrb/p/17004392.html

Autoware定位与建图模块(一)——定位