GNSS mapping: GNSS로 points map 만들기

Intro

자율주행을 위해서는 현재 차량의 위치를 지도 상에서 특정하는 localization이 필요하다. localization은 단순히 GNSS를 통해서도 수행할 수 있지만, 오차가 있을 수 있어 대부분은 points map과 LiDAR에서 받아온 3D points 정보를 align (matching)하여 localization을 수행한다. (LiDAR만 쓰는 것도 한계가 있어 GNSS, IMU 등을 종합하여 사용하기도 한다) LiDAR 기반 localization은 센서의 성능도 중요하겠지만, points map에 matching을 하는 것이기 때문에 map의 quality도 매우 중요하다. Autoware의 경우 NDT 알고리즘을 기반으로 한 NDT mapping 모듈을 제공하나, 직접 사용해본 결과 align이 깨지면 못 쓸 정도의 map이 되어버린다. (시간이 날 때 NDT 알고리즘에 대해서도 간단히 포스팅하도록 하겠다) 따라서 GNSS를 기반으로 한 mapping 모듈을 만들었는데, 사용법을 간단히 소개하겠다.

Usage

[Requirement]

• ROS
• ROS의 setup.bash가 sourcing되어있다고 가정
  (ex: source /opt/ros/melodic/setup.bash)

[Input topic]

• pointcloud (sensor_msgs::PointCloud2 type)
• gnss (geometry_msgs::PoseStamped type)

rosbag으로 녹화하거나 simulator 등에서 실시간으로 topic을 publish하는 방법으로 줄 수 있다. topic 이름은 launch 파일을 통해 설정할 수 있도록 하였다.

[실행]
public repo로 만들어서 clone한 후 build하면 된다.

git clone https://github.com/Spiraline/GNSS-mapping.git
cd GNSS-mapping
catkin_make
source devel/setup.bash
roslaunch launch/gnss_mapping.launch

[Parameter]

• gnss_topic: gnss topic 이름
• points_topic: points topic 이름
• map_topic: map topic 이름 (파일 형태로 사용할 경우 뭐로 설정해도 상관없다.)
• voxel_leaf_size: points data를 voxel grid filter로 한번 필터링한 후 합치는데, 이 때 필터링하는 정도이다. 0.0이면 필터링을 아예 하지 않고, 값이 클수록 필터링을 더 많이 한다.
• min_scan_range, max_scan_range: points data 중 LiDAR를 기준으로 한 거리가 이 사이에 있는 data만 사용한다.
• use_ndt: NDT 알고리즘을 사용할지에 대한 flag다. 개인적으로는 사용하지 않는 것을 추천한다.
• use_gnss_ori: GNSS의 방향 정보를 사용할 것인지에 대한 flag이다. topic 내의 방향 값이 정확하다면 사용하는 것을 추천한다.
• min_add_scan_shift: 이전에 merge한 points의 위치에서부터 이 값만큼 떨어진 후에 다음 points을 map에 합친다.
• output_path: map을 저장할 위치이다.
• tf_XX: GNSS와 LiDAR 간의 위치나 방향이 다를 경우 tf에 넣어줘야 한다.

[points_map 저장법]

rosparam set save_map $(voxel leaf size)

최종적으로 맵을 저장하기 전에 voxel grid filter로 다시 필터링한 후 저장한다. save_map이라는 parameter의 값을 지정해주면 저장한 후 다시 save_map의 값은 -1로 돌아간다.

Example

위 launch 파일은 SVL Simulator를 target으로 만들었기 때문에 SVL의 GNSS sensor가 publish하는 nmea_sentence를 우리가 잘 알고 있는 x, y, z, quaternion으로 변환하는 gnss_calibrator node를 포함한다. 그런데 SVL이 6월부터 지원을 종료한다고 하여 (SVL 사용법을 적으려다가 지원을 종료한다고 하여 따로 적지 않기로 했다) 일단은 예시 rosbag으로 사용하는 법을 적는다. 실제 주행 상황처럼 LiDAR의 위치가 틀어져있는 rosbag을 준비했다. 아래 그림처럼 틀어져 있다.

• x: 5 (m)
• y: -3 (m)
• yaw: 45 (deg)

1. gnss_mapping launch 파일의 parameter를 설정한다.

   <!-- TF from gnss to localizer (LiDAR) -->
     <arg name="tf_x" default="5.0" />
     <arg name="tf_y" default="-3.0" />
     <arg name="tf_z" default="0.0" />
     <arg name="tf_roll" default="0.0" />
     <arg name="tf_pitch" default="0.0" />
     <arg name="tf_yaw" default="-0.785" />

   그 외의 parameter도 해보고 싶다면 바꿔도 좋다.

2. gnss_mapping launch 파일을 실행한다.

   cd gnss_mapping
   roslaunch launch/gnss_mapping.launch

   roslaunch gnss_mapping gnss_mapping.launch로 실행하는 방법도 있으나, 이 경우 build (catkin_make)를 해야 바꾼 launch 파일이 실행되는 경로에 갱신된다.

3. rosbag을 실행한다. launch 파일을 실행하면서 roscore가 이미 실행됐으므로 roscore를 킬 필요는 없다.

   cd example
   rosbag play cubetown.bag

   해당 rostopic에는 아래의 두 topic이 녹화되어 있다.

   • points_raw: LiDAR의 points data
   • gnss_pose: x, y, z, quaternion으로 저장된 GNSS data

   gnss_pose를 echo해보면 아래처럼 나오는 것을 확인할 수 있다.
   

4. (Optional) Rviz에서 frame을 map으로 바꾸면 진행상황을 확인할 수 있다. (topic 이름은 points_map인데 일반적으로 기본 setting에 visualize하도록 들어가 있다.)

   

   orientation을 사용하는 경우 아래처럼 map이 만들어진다. github 용량제한 때문에 100MB 이하의 rosbag을 만드느라 주행을 빠르게 해서 map의 quality가 매우 좋다고는 할 수 없겠으나 지형 지물들은 모두 잘 나온 것을 확인할 수 있다.
   

5. rosbag의 실행이 끝나면 map을 저장한다.

   rosparam set save_map 0.1

   아래의 메시지가 뜬다면 해당 경로에 pcd 파일이 저장된 것이다.