仿真实现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文件中

修改为自己小车的话题

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

修改保存路径

  # Export settingssavePCD: 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: 0localizer: 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);  //defaultros::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数据的问题，整体上达到定位的要求。