Autoware.ai代码解读::NDT模块-1

Autoware.ai代码解读::NDT模块-1

在Autoware.ai的官方rosbag例子中,采用了NDT定位的算法进行演示,先看演示效果

Youtube

https://www.youtube.com/watch?v=OWwtr_71cqI&t=217s

例子中使用的定位launch文件如下

my_localization.launch

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
<launch>
<!-- setting path parameter -->
<arg name="get_height" value="true" />

<!-- Setup: -->
<include file="$(find runtime_manager)/launch_files/setup_tf.launch">
<arg name="x" value="1.2" />
<arg name="y" value="0.0" />
<arg name="z" value="2.0" />
<arg name="yaw" value="0.0" />
<arg name="pitch" value="0.0" />
<arg name="roll" value="0.0" />
<arg name="frame_id" value="/base_link" />
<arg name="child_frame_id" value="/velodyne" />
<arg name="period_in_ms" value="10"/>
</include>
<include file="$(find vehicle_description)/launch/vehicle_model.launch" />

<!-- points downsampler -->
<include file="$(find points_downsampler)/launch/points_downsample.launch" />

<!-- nmea2tfpose -->
<include file="$(find gnss_localizer)/launch/nmea2tfpose.launch"/>

<!-- ndt_matching -->
<include file="$(find lidar_localizer)/launch/ndt_matching.launch">
<arg name="get_height" value="$(arg get_height)" />
</include>

</launch>

主要启动launch/ndt_matching.launch文件

launch/ndt_matching.launch文件如下

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
<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="1" />
<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" />

<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)" />
<remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
</node>
</launch>

可以看到,主要是一些参数的设置

接着是启动ndt_matching这个节点

现在来到ndt_matching节点的入口main函数: