Cartographer/Hector SLAM 사용법

- 전제 조건

• ROS Noetic 설치(Ubuntu 20.04에서 진행. 다른 version의 경우 해당 ubuntu version에 맞는 ROS 설치)
  • http://wiki.ros.org/noetic/Installation/Ubuntu

noetic/Installation/Ubuntu - ROS Wiki

 

• turtlebot3 설치
  • https://github.com/ROBOTIS-GIT/turtlebot3

GitHub - ROBOTIS-GIT/turtlebot3: ROS packages for Turtlebot3

 

• turtlebot3_msgs 설치
  • https://github.com/ROBOTIS-GIT/turtlebot3_msgs

GitHub - ROBOTIS-GIT/turtlebot3_msgs: ROS msgs package for TurtleBot3

 

• map_server package 설치(다른 version의 경우 noetic을 본인의 version에 맞게 입력 

$ sudo apt install ros-noetic-map-server

 

 

1. Hector SLAM

• Hector SLAM : 2D LiDAR의 적은 계산량을 통해 2D Occupancy grid를 빠르게 생성하는 방식. 완전한 SLAM 접근 방식은 아니며 Loop Closure를 수행하지 않는다
  • Loop Closure Detection : 로봇의 이동 궤적상에서 현재의 위치가 이전에 방문했던 위치인지를 판단하는 것으로, 검출된 결과를 환경 맵 최적화 단계에서 제약조건으로 활용하도록 하여 SLAM 알고리즘의 로봇 표류 문제를 해결한다.
• Odometry 입력 값을 필요로 하지 않아 성능을 낮추는 단점이 되며 geometric constraints가 충분히 많지 않은 지형에 대해서는 오류가 많이 발생한다
• hector_slam package는 map을 learning하고 laser scanner frame rate에서 robot의 2D pose를 simultaneously하게 estimate하는데 사용
• hector_mapping의 frame명과 option을 알맞게 설정해야 함

 

• 일반적인 two wheeled robot의 standard coordinate frames

출처 : https://www.ros.org/reps/rep-0105.html

 

 

• 위 그림은 일반적인 Two Wheel Robot이 주행할 때의 관심 potential frames와 Robot의 Roll, Pitch, Yaw를 Robot의 simplified 2D view로 표현해놓은 것
• base_link frame을 laser_link frame으로 framsformation 하는 사진

 

- 새로운 localization component 작성에 필요한 frame들과 robot의 mobile base에 사용될 수 있는 frame에 대한 예시

• Coordinate Frames
• base_link : Robot의 base에 rigidly하게 붙는 coordinate frame으로, 임의의 위치나 방향으로 부착할 수 있으며, 모든 hardware platform에 대하여 명확한 기준점을 제공하는 다른 위치가 있다
• base_stabilized frame과 비교하여  roll, pitch angle 정보를 제공한다. (roll/pitch motion을 나타내지 않는 platform에서는 base_link와 base_stabilized frame이 동일하다)
• odom : World-fixed frame으로, 이 frame 내에서 robot의 pose는 시간이 지남에 따라 drift되어 따라서 장기적으로 사용하기에는 어려움이 있으나 연속적인 frame이므로 frame 내의 robot의 pose가 discrete한 jump없이 부드럽게 회전한다.
• 보통 바퀴나 visual, 혹은 inertial 측정 unit으로 부터 받은 odometry source를 통해 odom frame이 계산된다
  • Odometry : 주행기록계라는 뜻으로 시간에 따른 위치 변화를 추정하는 개념으로, GPS와 같은 절대적 위치가 아닌 출발 지점으로부터의 상대적인 위치를 추정한다.
  • Motor의 Encoder의 회전수를 통해 거리를 측정하고, IMU센서를 통해 기울기를 측정하여 로봇의 위치를 추정한다
• map : odom과 마찬가지로 world-fixed frame으로, Z축이 위쪽을 향한다. 이 frame에서의 robot의 pose는 시간이 지나도 drift되어서는 안된다. 불연속적인 frame이라 frame 내의 robot의 pose는 어느때나 discrete하게 jump될 수 있다.
• 보통 localization component가 계속적으로 sensor 측정 값을 토대로 robot의 pose를 계산하므로 drift를 없애나 새로운 sensor의 정보가 입력되면 discrete한 jump가 발생한다
• 장기적으로 사용하기 유용하나 position estimator에서 discrete한 jump로 인해 local sensing과 acting의 성능을 저하시킨다
• base_footprint : 높이 정보를 제외한 robot의 2D pose를 나타낸다 (위치, 방향)
• base_stabilized : map/odom layer와 관련된 robot의 높이 정보를 나타낸다

 

 

- Hector SLAM 수행

$ cd ~/catkin_ws/src
$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

• Hector Slam package를 clone

$ cd ~/catkin_ws/src/hector_slam/hector_mapping/launch/mapping_default.launch

• Hector_SLAM의 hector_mapping node의 launch file을 염

• odom frame 없이 mapping을 진행 할 것이므로 transformation을 map->base_link로 바로 publish함 (line 5~6)

• tf parameter 수정 (lline 54)

행

$ cd ~/catkin_ws/src/hector_slam/hector_slam_launch/launch
$ gedit tutorial.launch

• hector_slam_launch directory로 이동하여 tutorial.launch 파일 실행

• use_sim_time 사용 X

 

$ cd ~/catkin_ws
$ catkin_make

• 위 과정 수행 후 package compile

 

• Melodic 버전에선 위와 같은 오류가 발생함

 

$ cd /usr/include
$ sudo ln -s opencv4/ opencv
$ cd ~/catkin_ws
$ catkin_make

• 위 오류를 해결하기 위해 opencv4를 설치하고 package를 다시 build
• 오류 해결

 

$ cd ~/catkin_ws
$ sudo chmod 666 /dev/ttyUSB0
$ roslaunch rplidar_ros rplidar.launch

• Port 권한 부여 후
• Raspberry Pi에서 RPLidar 실행

$ roslaunch hector_slam_launch tutorial.launch

• 노트북에서 Hector SLAM 실행
• 위 명령어는 아래 3개의 node들을 launch함
  • hector_mapping : odometry 없이 SLAM을 수행할 수 있게 함
  • hector_trajectory_server : tf data로부터 extracted된 tf의 이동경로를 추적하고 해당 data를 service와 topic을 통해 접근 할 수 있게 함
  • hector_geotiff : occupancy grid map를 저장하고 robot의 trajectory 및 관심 객체 data를 RoboCup Rescue 호환 GeoTiff image에 저장하는데 사용할 수 있는 node를 제공함
  • occupancy grid map은 자료가 격자의 형태로 전달되어지는 지도를 말한다

 

• Lidar를 들고 매우 천천히 움직여 map을 생성한다

 

 

- Map 저장 방식 1)

$ rostopic pub syscommand std_msgs/String "savegeotiff"

• Mapping 완료 후 CTRL + C 키를 눌러 종료시키고 아래 명령 입력

$ Go to ~/catkin_ws/src/hector_slam/hector_geotiff/maps.

• 아래 그림처럼 생성된 map(hector_slam_map_14:34:07.tfw)을 확인할 수 있다

출처 : https://automaticaddison.com/how-to-build-an-indoor-map-using-ros-and-lidar-based-slam/#Run_rviz

 

 

 

- Map 저장 방식 2) : map_server package를 통해 map 저장. (map data를 yaml과 pgm format file로 저장)

 

$ sudo apt-get install ros-noetic-map-server

• map_server package install

$ mkdir ~/catkin_ws/maps

• catkin workspace에 map을 저장할 folder 생성
• 위에서 했던대로 RPLidar, Hector SLAM node를 launch하여 mapping 진행

$ cd ~/catkin_ws/maps
$ rosrun map_server map_saver -f my_map

• map directory로 이동 후 map_server node를 실행시켜 map 저장
• 'my_map'은 이름 예시 
• my_map.pgm과 my_map.yaml 파일이 생성됨

 

$ roscore

• ROS Master 실행

 

$ cd ~/catkin_ws/maps
$ rosrun map_server map_server my_map.yaml

• 새로운 terminal 창에서 map directroy로 이동 후  위 명령어 실행

 

$ rviz

• rviz 실행 후 왼쪽 아래의 'Add'를 클릭하여 Map Display를 추가함
• Topic창의 Map의 하위 목록에 있는 /map 선택
• rviz 창에서 앞서 저장한 map을 볼 수 있음

 

- 저장한 map을 png format으로 변환

$ sudo apt-get install imagemagick
$ convert my_map.pgm my_map.png

• Map을 png 이미지 file로 변환하기 위해 imagemagick 설치 후 실행
• my_map.png 파일이 생성됨

 

$ sudo apt-get update
$ sudo apt-get install gimp

• Map을 수정하기 위해 gimp 설치

$ gimp

• gimp 실행

 

 

2. Cartographer SLAM

 

- Cartographer 설치

 

$ ssh host_name@ip_address

• ROS Master 역할을 할 PC에서 Mapping을 진행할 robot에(remote PC) ssh로 연결
• Cartographer 설치는 전부 remote PC에서 진행

 

$ sudo apt update
$ sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow

• wstool, rosdep, ninja 설치
  • wstool : Multiple version-control system에서 project의 workspace를 유지하는데 사용되는 command-line tools
  • ninja : 속도에 중점을 둔 소형 build system. 다른 build system과 달리 상위 level build system에서 입력 파일을 생성하도록 설계되었으며 최대한 빨리 build를 실행하도록 설계됨

 

$ mkdir ct_ws
$ cd ~/ct_ws
$ wstool init src
$ wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
$ wstool update -t src

• Cartographer를 위한 workspace생성 후 새로운 catrographer_ros workspace 생성

 

$ sudo rosdep init	-> ROS 설치시 이 명령어를 실행했을 경우 오류 발생(무시하면 됨)
$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

• cartographer_ros dependencies 설치를 위해 rosdep 실행

 

ERROR: the following packages/stacks could not have their rosdep keys 
resolved to system dependencies: cartographer: [libabsl-dev] defined as 
"not available" for OS version [focal]

• 위의 명령어들 중 마지막을 실행한 후 위와 같은 error 발생
• cartographer의 package.xml에서 아래 줄을 주석처리

 

$ cd src/cartographer/scripts
$ sudo ./install_abseil.sh

• abseil 수동 설치
  • abseil : Google 내부의 코드에서 가장 핵심적인 부분을 추려 만든 open source C++ library collection

 

 

$ cd ~/ct_ws
$ catkin_make_isolated --install --use-ninja

• Build, install
• Master branch의 최신 HEAD에서 cartographer가 build됨
• 특정 버전을 원하면 cargrapher_ros.rosinstall에서 변경 가능

 

 

- Cartographer demo bag 실행

 

$ source install_isolated/setup.bash

• Cartographer 실행 전 ROS 환경을 설정해야 함

 

 

 

- Deutsches Museum 예제

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
$ roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

• 2D backpack demo 설치, 실행

 

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag
$ roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag

• 3D backpack demo 설치, 실행

 

 

- Pure localization

• Pure localization은 2개의 다른 bag을 사용
• 첫번째는 map 생성, 두번째는 pure localization을 실행하기 위해 사용

 

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag

• Deutsche Museum으로부터의 2D bags 설치

 

$ roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
$ roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
   bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag

• cartographer_offline_node가 종료될 때 까지 기다린 후 Map 생성
• Pure localization 실행

 

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag
$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag

• Deutsche Museum으로부터의 3D bags 설치

 

$ roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag
$ roslaunch cartographer_ros demo_backpack_3d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \
   bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag

• 2D와 동일 과정

 

 

 

- Static landmarks

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/mir/landmarks_demo_uncalibrated.bag

• Landmarks example bag 설치

 

$ roslaunch cartographer_mir offline_mir_100_rviz.launch bag_filename:=${HOME}/Downloads/landmarks_demo_uncalibrated.bag

• Landmarks demo 실행

 

 

• 이외의 예제들은 아래의 링크에서 확인할 수 있다
• https://google-cartographer-ros.readthedocs.io/en/latest/demos.html#deutsches-museum-1

Running Cartographer ROS on a demo bag — Cartographer ROS documentation

 

 

-Cartographer를 통한 실제 Mapping 진행

 

$ cd ct_ws
$ mkdir src
$ cd src
$ catkin_create_pkg slam roscpp

• Cartographer workspace로 이동 후 roscpp dependency를 가지는 'slam' package 생성

 

$ cd slam
$ rm -rf include/ src

• slam package로 이동 후 불필요한 폴더들(include, src) 삭제

 

$ mkdir launch
$ mkdir lua

• launch, lua 폴더 생성

 

$ cd ct_ws/src/cartographer_ros/cartographer_ros/launch
$ cp backpack_2d.launch /home/lim/ct_ws/src/slam/launch/my_robot.launch
$ cd ..
$ cd configuration_files
$ cp backpack_2d.lua /home/lim/ct_ws/src/slam/lua/my_robot.lua

• 새로운 terminal 창을 연 후 Cartographer의 launch file과 lua file을 slam package에 복사

 

$ cd ct_ws/src/slam/launch
$ gedit my_robot.launch

• launch file을 현재 robot에 맞게 변형

 

왼쪽 : 수정 전, 오른쪽 : 수정 후

<launch>
  <node name="cartographer_node" pkg="cartographer_ros"
    type="cartographer_node" args="
      -configuration_directory $(find slam)/lua
      -configuration_basename my_robot.lua"
      output="screen">
  <remap from="scan" to ="scan" />
  <remap from="odom" to ="odom_data_quat" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
    type="cartographer_occupancy_grid_node" args="-resolution 0.02" />
</launch>

 

• tf는 이미 정의해 놓았으므로 (ris_bot.launch file 내부) urdf 관련 내용 삭제
• lua file은 새로 만든 slam package 내부의 lua file을 참고하도록 변경
• remap은 현재 사용하는 sensor에 맞게 변경 (from은 센서 종류, to는 topic 이름)
  • Lidar :scan
  • Odometry : odom
• resolution은 map의 해상도를 의미. 원하는 값으로 수정

 

$ cd ..
$ cd lua
$ gedit my_robot.lua

• lua file 수정

 

왼쪽 : 수정 전, 오른쪽 : 수정 후

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_foorprint",
  published_frame = "base_foorprint",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = true,
  use_pose_extrapolator = true,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 10,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.03
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
POSE_GRAPH.optimization_problem.odometry_translation_weight = 0.5
POSE_GRAPH.optimization_problem.odometry_roration_weight = 0.5
return options

 

 

$ cd {workspace_where_cartographer_installed}
$ source devel_isolated/setup.bash
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ..
$ catkin_make

• 새로 만든 package에서 Cartographer package를 search 할 수 있도록 함
• 이 과정 이후 내 workspace에서 source devel/setup.bash를 입력하면 Cartographer package까지 search됨

 

- Cartographer SLAM 실행

 

# Remote PC
$ cd catkin_ws
$ source devel/setup.bash
$ roslaunch navstack forCarto.launch

• Remote PC(Jetson, Raspberry Pi...)에서 실행
• 각 sensor들의 구동을 위한 launch file 실행

 

# Remote PC
$ cd ct_ws

$ source devel/setup.bash
or
$ source devel_isolated/setup.bash

$ roslaunch slam my_robot.launch

• Cartographer가 설치된 workspace로 이동 후 Cartographer SLAM 실행

 

# Master PC
$ rviz

• roscore를 실행시키는 ROS_MASTER_URI에 해당하는 PC에서 rviz 창을 켬
• 좌측 하단의 'Add'를 클릭한 후 Map Topic 추가
• 그리고 TF, PointCloud2를 추가한 후 PointCloud2의 Topic을 /scan_matched_points2로 지정하고 크기를 눈에 잘 보일 정도로 키움 (0.05 정도)

 

# Master PC
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

• turtlebot3 teleop node를 실행시킨 후 키보드 입력으로 로봇을 천천히 이동시키며 mapping 진행

 

# Remote PC
$ mkdir ~/catkin_ws/maps
$ cd ~/catkin_ws/maps
$ rosrun map_server map_saver -f map_name

• catkin workspace에 map을 저장할 folder를 생성한 후 이동
• map_server node를 실행시켜 작성한 map 저장
• map_name.pgm과 map_name.yaml file이 생성됨

 

- Map 관련 Parameter

• Mapping 진행 후 Map을 저장하면 해당 Map에 대한 YAML/PGM 2개의 File들이 생성된다

- YAML File

image: test_Hector.pgm         
resolution: 0.050000           
origin: [-51.224998, -51.224998, 0.000000]
negate: 0                       
occupied_thresh: 0.65           
free_thresh: 0.196

• image : 생성된 Map의 name
• resolution : Map의 해상도 (meter / pixel)
• origin : Map의 좌측 하단 Pixel의 2D 좌표. 세번째 숫자는 회전을 의미
• negate : Map의 색 결정. Dafault는 흰색이 Free Space, 검은색이 Occupied Space
• occupied_thresh : Occupied Space를 결정하는 Threshold Value. 이 값보다 크면 Occupied Space로 간주함
• free_thresh : Free Space를 결정하는 Threshold Value. 이 값보다 크면 Free Space로 간주함

 

- PGM(Portable Gray Map) File (Image File)

• Robot의 환경에 대한 각 Pixel의 Occupancy Status를 묘사
• 흰색은 자유 공간, 검은색은 점유 공간, 회색은 알려지지 않은 공간을 의미
• ROS상에서의 Message 통신 시 Occupancy는 0~100 사이의 정수로 표현됨
  • 0 : 완전한 Free Sapce
  • 100 : 완전한 Occupied Space
  • -1 : Unknown Space

 

• Map 저장에 사용하는 map_server Package는 map_server node 또한 제공하는데, 이 node는 저장공간으로부터 Map File을 읽고 ROS Service를 통해 해당 File을 요청하는 다른 node에 제공함
  • Service를 통해 Map Data를 가진 ROS Messgae를 전달
  • static_map (nav_msgs/GetMap) : Map의 Occupancy Data를 해당 Service를 통해 제공
  • Topic을 통해 Map Data를 가진 ROS Messgae를 전달
  • *map_metadata (nav_msgs/MapMetaData) : Map의 metadata를 해당 Topic을 통해 제공
  • *map (nav_msgs/OccupancyGrid) : Map의 Occupancy Data를 해당 Topic을 통해 제공

 

 

---

참고 자료 :

https://automaticaddison.com/how-to-build-an-indoor-map-using-ros-and-lidar-based-slam/

How to Build an Indoor Map Using ROS and LIDAR-based SLAM – Automatic Addison

 

http://jinyongjeong.github.io/2017/02/21/lec10_Grid_map/

[SLAM] Occupancy Grid Maps · Jinyong

 

http://wiki.ros.org/hector_slam/Tutorials/SettingUpForYourRobot

hector_slam/Tutorials/SettingUpForYourRobot - ROS Wiki

 

https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html

Compiling Cartographer ROS — Cartographer ROS documentation

http://wiki.ros.org/wstool

wstool - ROS Wiki

https://www.youtube.com/watch?v=sbiLDPYsp20