仿真实现lio_sam建图和ndt_matching定位

---

一、仿真环境

使用现有开源的仿真环境—从零开始搭建一台ROS开源迷你无人车，作者已经配置好小车模型以及gazebo环境，imu频率已改为200HZ，文中也有详细的说明，这里就不再介绍。

启动仿真

roslaunch steer_mini_gazebo steer_mini_sim_sensors_VLP16_lio_sam.launch 

话题如下：

/ackermann_steering_controller/cmd_vel
/ackermann_steering_controller/odom
/clock
/gains/left_rear_joint/parameter_descriptions
/gains/left_rear_joint/parameter_updates
/gains/right_rear_joint/parameter_descriptions
/gains/right_rear_joint/parameter_updates
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/imu/data
/joint_states
/rosout
/rosout_agg
/tf
/tf_static
/velodyne_points

控制小车移动，打开一个终端，录制所有话题（也可录制需要的话题），会自动在终端所在的文件夹下根据当前时间产生bag文件。

rosbag record -a

二、lio_sam建图

lio_sam的安装测试可参考Lego-LOAM和LIO_SAM的使用及地图的处理

1.修改配置文件

在LIO-SAM/config下的params.yaml文件中

修改为自己小车的话题

  # Topics
  pointCloudTopic: "/velodyne_points"              
  imuTopic: "/imu/data"                         
  odomTopic: "/odometry/imu"                   
  gpsTopic: "odometry/gpsz"                   

修改保存路径

  # Export settings
  savePCD: true                              
  savePCDDirectory: "/Downloads/"        

修改imu到雷达的坐标变换

  # Extrinsics: T_lb (lidar -> imu)
  extrinsicTrans: [0.0, 0.0, 0.0]
  extrinsicRot: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]
  extrinsicRPY: [1, 0, 0,
                  0, 1, 0,
                  0, 0, 1]

其它的保持默认即可。

2.开始建图

	roslaunch lio_sam run.launch 
	rosbag play your.bag 

保存的PCD—GlobalMap.pcd

三、ndt_matching定位

1.新建启动文件

使用Autoware.ai中的ndt_matching定位模块。为了适配前方，需要对应仿真的数据进行如下修改

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/config路径：
新建headless_setup_steer_mini.yaml 内容如下：

tf_x: 0
tf_y: 0
tf_z: 0.115
tf_yaw: 0
tf_pitch: 0
tf_roll: 0

localizer: velodyne
use_sim_time: false

.autoware/data/tf 路径：
新建tf_steer_mini.launch

<launch>
<!-- worldからmapへのtf -->
<node pkg="tf2_ros"  type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map" />
<!-- mapからmobilityへのtf -->
<!-- <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 0 0.115 0 0 0 /base_link /velodyne" />
</launch>

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo路径下：
新建my_map_steer_mini.launch内容如下

<launch>
  <rosparam command="load" file="$(find autoware_quickstart_examples)/config/headless_setup_steer_mini.yaml" />

  <include file="$(env HOME)/.autoware/data/tf/tf_steer_mini.launch"/>
  
  <node pkg="map_file" type="points_map_loader" name="points_map_loader" args="noupdate $(env HOME)/.autoware/data/map/pointcloud_map/GlobalMap.pcd"/>


</launch>

autoware.ai/src/autoware/core_perception/lidar_localizer/launch路径下：
新建ndt_matching_steer_mini.launch内容如下：

<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="0" />
  <arg name="use_odom" default="true" />
  <arg name="use_imu" default="true" />
  <arg name="imu_upside_down" default="false" />
  <arg name="imu_topic" default="/imu/data" />
  <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.cpp中的里程计接收话题

  // ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", _queue_size * 10, odom_callback);  //default
  ros::Subscriber odom_sub = nh.subscribe("/ackermann_steering_controller/odom", _queue_size * 10, odom_callback);

autoware.ai/src/autoware/core_perception/points_downsampler/launch路径下：
新建points_downsample_steer_mini.launch

<launch>
  <arg name="sync" default="false" />
  <arg name="node_name" default="voxel_grid_filter" />
  <arg name="points_topic" default="/velodyne_points" />
  <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>

autoware.ai/src/autoware/documentation/autoware_quickstart_examples/launch/rosbag_demo路径下：
新建my_localization_steer_mini.launch

<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_steer_mini.launch" />

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

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

</launch>

2.启动

roslaunch autoware_quickstart_examples my_map_steer_mini.launch
roslaunch autoware_quickstart_examples my_localization_steer_mini.launch
rviz
给定初始位姿(必须)
rosbag play your.bag

效果图：

---

总结

以上实现了仿真场景下使用lio_sam建图以及ndt_matching定位的过程，仅仅作为测试使用，存在仿真场景较小，没有在机器人模型中添加gps数据的问题，整体上达到定位的要求。