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

转载自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。

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

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坐标计算。

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>

其中

• 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即可。