1. Cartographer란 무엇인가?
Cartographer는 Google에서 공개한 실시간 SLAM 라이브러리입니다.
Google은 2016년에 Cartographer를 오픈소스로 공개하면서, 이를 ROS를 지원하는 2D/3D 실시간 SLAM 라이브러리라고 소개했습니다.
Cartographer 공식 문서에서 Cartographer는 여러 플랫폼과 센서 구성에서 2D 및 3D 실시간 SLAM을 제공하는 시스템이라고 설명합니다.
이름 그대로 Cartographer는 원래 “지도 제작자”라는 뜻입니다.
ROS에서는 로봇이 움직이면서 주변을 측정하고, 그 결과를 지도 형태로 만드는 SLAM 시스템을 의미합니다.
2. Cartographer의 등장 배경
초기 모바일 로봇 SLAM에서는 gmapping 같은 2D SLAM 패키지가 많이 사용되었습니다.
gmapping은 ROS 1 시절부터 교육용, 연구용으로 널리 쓰였고, 구조도 비교적 단순했습니다.
하지만 로봇 활용 범위가 넓어지면서 더 복잡한 요구가 생겼습니다.
더 큰 공간의 지도 작성
더 정확한 loop closure
2D뿐 아니라 3D SLAM 지원
다양한 센서 조합 지원
실시간 지도 작성
ROS와의 통합
Cartographer는 이런 요구를 배경으로 등장했습니다.
Google의 공개 글에서는 SLAM이 LiDAR, IMU, 카메라 같은 여러 센서 데이터를 조합해 센서의 위치와 주변 지도를 동시에 계산한다고 설명합니다.
또한 Cartographer가 실시간으로 전역적으로 일관된 지도를 만들 수 있고, loop closure를 지원한다고 소개하고 있습니다.
여기서 중요한 표현은 전역적으로 일관된 지도입니다.
로봇이 긴 복도를 한 바퀴 돌아 출발점 근처로 돌아왔다고 생각해 보겠습니다.
센서와 바퀴 odometry에는 항상 오차가 있습니다. 그래서 로봇은 실제로는 제자리로 돌아왔지만, 계산상으로는 조금 다른 위치에 있다고 판단할 수 있습니다.
Cartographer는 이런 상황에서 다음과 같은 보정을 수행합니다.
“아, 여기는 전에 봤던 장소와 같은 곳이구나.”
“그렇다면 지금까지 만든 전체 지도를 조금 보정해야겠구나.”
이 과정을 일반적으로 loop closure라고 합니다.
3. Cartographer가 하는 일
TurtleBot3에서 Cartographer가 하는 일을 단순화하면 다음과 같습니다.
1. /scan에서 LiDAR 데이터를 받는다.
2. /odom에서 로봇 이동 추정값을 받는다.
3. /tf에서 로봇과 센서 좌표 관계를 확인한다.
4. 현재 LiDAR 데이터와 이전 지도 데이터를 비교한다.
5. 로봇의 현재 위치를 추정한다.
6. 주변 벽과 장애물을 지도에 누적한다.
7. /map 토픽으로 지도를 발행한다.
그림으로 표현하면 다음 구조입니다.
TurtleBot3 Gazebo
├── /scan
├── /odom
└── /tf
↓
Cartographer
↓
/map
↓
RViz2
Cartographer는 단순히 /scan을 그림으로 찍는 것이 아닙니다.
로봇이 이동하면서 얻은 여러 LaserScan을 서로 맞추고, odometry 오차를 줄이며, 전체 지도를 자연스럽게 연결합니다.
4. Cartographer의 주요 특징
1) 실시간 SLAM
Cartographer는 로봇이 움직이는 동안 지도를 실시간으로 생성합니다.
즉, 먼저 데이터를 저장하고 나중에 지도를 만드는 방식만 가능한 것이 아니라, TurtleBot3가 움직이는 동안 RViz2에서 지도가 점점 만들어지는 모습을 볼 수 있습니다.
로봇 이동 → LiDAR 측정 → 지도 생성
이 흐름을 매우 직관적입니다.
2) 2D와 3D SLAM 지원
Cartographer는 2D와 3D SLAM을 모두 지원하는 시스템입니다.
다만 이번 강의에서는 TurtleBot3 Burger의 2D LiDAR 시뮬레이션을 사용하므로 2D SLAM만 다룹니다.
3D LiDAR, 카메라 기반 SLAM, RGB-D SLAM은 이 글의 범위를 벗어납니다.
3) ROS와 통합 가능
Cartographer 자체는 C++ 기반 SLAM 시스템입니다.
하지만 cartographer_ros 패키지를 통해 ROS 토픽, TF, launch 파일과 연결해 사용할 수 있습니다.
TurtleBot3에서는 이미 turtlebot3_cartographer 패키지가 준비되어 있어, 복잡한 설정 없이 다음 명령으로 실행할 수 있습니다.
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True
4) Loop Closure 지원
Cartographer의 중요한 장점 중 하나는 loop closure입니다.
로봇이 같은 장소를 다시 방문했을 때, Cartographer는 이전에 본 공간과 현재 센서 데이터를 비교해 전체 지도를 보정할 수 있습니다.
예를 들어 로봇이 사각형 복도를 한 바퀴 돌았다고 가정합니다.
출발점 → 복도 → 코너 → 복도 → 다시 출발점 근처
odometry만 믿으면 오차가 계속 누적됩니다.
그래서 마지막에 출발점으로 돌아왔을 때 지도 벽이 어긋날 수 있습니다.
Cartographer는 같은 장소를 다시 인식하면 다음처럼 전체 지도를 보정합니다.
이전에 봤던 벽과 현재 LiDAR 데이터가 비슷하다.
그러면 이 위치는 같은 장소일 가능성이 높다.
전체 경로와 지도를 다시 맞춰야 한다.
이 기능 덕분에 비교적 큰 공간에서도 지도 품질을 높일 수 있습니다.
7. Cartographer의 장점과 단점
장점
| 실시간 지도 생성 | 로봇이 움직이는 동안 바로 지도 확인 가능 |
|---|---|
| 2D/3D 지원 | 다양한 센서 구성에 대응 가능 |
| loop closure | 같은 장소 재방문 시 지도 오차 보정 가능 |
| ROS 연동 | ROS 토픽, TF, RViz와 함께 사용 가능 |
| TurtleBot3 예제 존재 | 교육용 실습 구성이 쉬움 |
Cartographer는 특히 LiDAR 기반 SLAM 개념을 설명하기 좋은 도구입니다.
TurtleBot3와 함께 사용하면 /scan, /odom, /tf, /map의 관계를 한 번에 보여줄 수 있습니다.
단점
| 설정 파일이 어렵다 | .lua 설정 파일 구조가 초보자에게 낯설다 |
|---|---|
| 튜닝 난이도 있음 | 센서, 속도, 공간 구조에 따라 파라미터 조정 필요 |
| 시스템 이해 필요 | TF, odometry, scan frame 관계가 맞아야 정상 동작 |
| 현재 신규 개발은 활발하지 않음 | 공식 저장소 기준 새 기능 개발은 중단 상태에 가깝다 |
Cartographer GitHub 저장소에는 “Cartographer is no longer actively maintained”라고 명시되어 있고, ROS용 fork도 제한적으로 유지된다고 안내하고 있습니다.
따라서 실무 프로젝트에서 새로 SLAM 시스템을 선택할 때는 Cartographer만 고집하기보다는 slam_toolbox, RTAB-Map, LIO-SAM, FAST-LIO 같은 다른 SLAM 도구도 함께 검토하는 것이 좋습니다.
하지만 교육용으로는 여전히 의미가 있습니다.
TurtleBot3 예제가 잘 되어 있고,
ROS 2 Humble에서 설치가 쉽고,
LiDAR SLAM 구조를 이해하기 좋기 때문입니다.
8. Python으로 간단한 Wall Following 노드 만들기
1) TurtleBot3 Gazebo 실행
첫 번째 터미널에서 TurtleBot3 Gazebo 월드를 실행합니다.
sl
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
실행되면 Gazebo 창에 TurtleBot3와 실습용 월드가 나타납니다.
토픽이 정상적으로 나오는지 확인합니다.
ros2 topic list
다음 토픽이 보여야 합니다.
/cmd_vel
/odom
/scan
/tf
/tf_static
라이다 데이터 확인:
ros2 topic echo /scan
ranges 배열이 계속 출력되면 라이다 시뮬레이션이 정상입니다.
2) Cartographer 실행
두 번째 터미널에서 Cartographer를 실행합니다.
sl
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True
RViz가 실행되면서 TurtleBot3 주변 지도가 생성되기 시작합니다.
Cartographer는 로봇이 움직여야 지도를 만듭니다.
즉, Gazebo만 켜져 있고 로봇이 멈춰 있으면 맵이 거의 만들어지지 않습니다.
그래서 이번 실습에서는 직접 키보드 조종 대신 Python wall following 노드로 로봇을 움직입니다.
3) Wall Following 알고리즘 개념
라이다는 로봇 주변 360도 거리를 배열로 제공합니다.
이번 예제에서는 세 방향만 사용합니다.
front : 로봇 정면 거리
front_right : 로봇 오른쪽 앞 대각선 거리
right : 로봇 오른쪽 거리
제어 규칙은 단순합니다.
1. 정면이 너무 가까우면 왼쪽으로 회전
2. 오른쪽 벽이 너무 멀면 오른쪽으로 접근
3. 오른쪽 벽이 너무 가까우면 왼쪽으로 멀어짐
4. 적당한 거리면 직진
목표 벽 거리:
0.45 m
최소 정면 안전 거리:
0.55 m
이 정도 값이면 TurtleBot3 Burger 시뮬레이터에서 안정적으로 실습하기에 적절합니다.
4) Python wall follower 노드 작성
my_first_package 안에 Python 파일을 만듭니다.
cd ~/ros2_study/src/my_first_package/my_first_package
touch wall_follower_node.py
다음 코드를 작성합니다.
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
class WallFollowerNode(Node):
def __init__(self):
super().__init__('wall_follower_node')
self.scan_sub = self.create_subscription(
LaserScan,
'/scan',
self.scan_callback,
qos_profile_sensor_data
)
self.cmd_pub = self.create_publisher(
Twist,
'/cmd_vel',
10
)
self.target_wall_distance = 0.45
self.front_safe_distance = 0.55
self.forward_speed = 0.12
self.turn_speed = 0.45
self.get_logger().info('wall_follower_node started')
def scan_callback(self, msg: LaserScan):
front = self.get_sector_min_distance(msg, -10.0, 10.0)
front_right = self.get_sector_min_distance(msg, -55.0, -25.0)
right = self.get_sector_min_distance(msg, -100.0, -80.0)
cmd = Twist()
if front < self.front_safe_distance:
cmd.linear.x = 0.0
cmd.angular.z = self.turn_speed
state = 'TURN_LEFT_FRONT_OBSTACLE'
elif right > self.target_wall_distance + 0.12:
cmd.linear.x = self.forward_speed
cmd.angular.z = -0.25
state = 'TURN_RIGHT_FIND_WALL'
elif right < self.target_wall_distance - 0.12:
cmd.linear.x = self.forward_speed * 0.8
cmd.angular.z = 0.25
state = 'TURN_LEFT_TOO_CLOSE'
else:
cmd.linear.x = self.forward_speed
cmd.angular.z = 0.0
state = 'GO_STRAIGHT'
self.cmd_pub.publish(cmd)
self.get_logger().info(
f'state={state}, front={front:.2f}, front_right={front_right:.2f}, right={right:.2f}'
)
def get_sector_min_distance(self, msg: LaserScan, start_deg: float, end_deg: float) -> float:
start_rad = math.radians(start_deg)
end_rad = math.radians(end_deg)
start_index = int((start_rad - msg.angle_min) / msg.angle_increment)
end_index = int((end_rad - msg.angle_min) / msg.angle_increment)
start_index = max(0, min(start_index, len(msg.ranges) - 1))
end_index = max(0, min(end_index, len(msg.ranges) - 1))
if start_index > end_index:
start_index, end_index = end_index, start_index
sector_ranges = msg.ranges[start_index:end_index + 1]
valid_ranges = []
for distance in sector_ranges:
if math.isfinite(distance):
if msg.range_min <= distance <= msg.range_max:
valid_ranges.append(distance)
if len(valid_ranges) == 0:
return msg.range_max
return min(valid_ranges)
def main(args=None):
rclpy.init(args=args)
node = WallFollowerNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
stop_cmd = Twist()
node.cmd_pub.publish(stop_cmd)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
5) wall following 코드 해석
핵심은 scan_callback()입니다.
front = self.get_sector_min_distance(msg, -10.0, 10.0)
front_right = self.get_sector_min_distance(msg, -55.0, -25.0)
right = self.get_sector_min_distance(msg, -100.0, -80.0)
라이다 전체 데이터 중에서 정면, 오른쪽 앞, 오른쪽 방향만 잘라서 최소 거리를 구합니다.
if front < self.front_safe_distance:
cmd.linear.x = 0.0
cmd.angular.z = self.turn_speed
정면이 가까우면 전진하지 않고 왼쪽으로 회전합니다.
elif right > self.target_wall_distance + 0.12:
cmd.linear.x = self.forward_speed
cmd.angular.z = -0.25
오른쪽 벽이 너무 멀면 오른쪽으로 살짝 회전하면서 벽을 찾습니다.
elif right < self.target_wall_distance - 0.12:
cmd.linear.x = self.forward_speed * 0.8
cmd.angular.z = 0.25
오른쪽 벽이 너무 가까우면 왼쪽으로 살짝 회전하면서 벽과 거리를 벌립니다.
else:
cmd.linear.x = self.forward_speed
cmd.angular.z = 0.0
적당한 거리면 그대로 직진합니다.
각도를 ranges 배열 인덱스로 변환합니다.
start_index=int((start_rad-msg.angle_min)/msg.angle_increment)
end_index=int((end_rad-msg.angle_min)/msg.angle_increment)
LaserScan 메시지의 거리 데이터는 msg.ranges 배열 안에 들어 있습니다.
예를 들어 라이다 데이터는 이런 식입니다.
msg.ranges= [
1.2,1.3,1.4,1.5, ...
]
문제는 우리가 원하는 것은 “오른쪽 90도 방향 거리”인데, 실제 데이터는 “배열의 몇 번째 값”으로 저장되어 있다는 점입니다.
그래서 각도를 배열 인덱스로 바꿔야 합니다.
ROS 2 LaserScan에는 이런 정보가 들어 있습니다.
angle_min : ranges[0]이 의미하는 시작 각도
angle_max : ranges[-1]이 의미하는 마지막 각도
angle_increment : ranges 배열 한 칸당 증가하는 각도
ranges : 실제 거리 배열
예를 들어 다음과 같다고 가정해 보겠습니다.
angle_min = -3.14 rad
angle_increment = 0.01745 rad
0도, 즉 0 rad가 몇 번째 인덱스인지 계산하면 다음과 같습니다.
index = (0 - (-3.14)) / 0.01745
index = 179.9
즉, 대략 ranges[180] 근처가 정면 방향입니다.
그래서 이 코드가 필요합니다.
start_index = int((start_rad - msg.angle_min) / msg.angle_increment)
계산된 인덱스가 항상 안전하다고 보장할 수는 없습니다.
start_index = max(0, min(start_index, len(msg.ranges) - 1))
end_index = max(0, min(end_index, len(msg.ranges) - 1))
예를 들어 msg.ranges 길이가 360이면 사용할 수 있는 인덱스는 다음 범위입니다.
0 ~ 359
그런데 계산 결과가 실수로 -5 또는 370이 될 수도 있습니다.
이 상태로 배열에 접근하면 에러가 납니다.
msg.ranges[370]
이런 경우 Python에서는 IndexError가 발생합니다.
그래서 인덱스를 안전한 범위로 제한합니다.
start_index = max(0, min(start_index, len(msg.ranges) - 1))
이 코드는 다음 의미입니다.
start_index가 0보다 작으면 0으로 만든다.
start_index가 마지막 인덱스보다 크면 마지막 인덱스로 만든다.
그 외에는 원래 값을 사용한다.
즉, 인덱스 안전장치입니다.
경우에 따라 start_index가 end_index보다 커질 수 있습니다.
if start_index > end_index:
start_index, end_index = end_index, start_index
예를 들어 사용자가 다음처럼 넣었다고 봅니다.
self.get_sector_min_distance(msg, 10.0, -10.0)
원래는 -10도 ~ +10도로 검사하고 싶었는데 순서를 반대로 넣은 상황입니다.
이 경우에도 함수가 죽지 않고 동작하도록 두 값을 바꿔줍니다.
start_index, end_index = end_index, start_index
즉, 항상 작은 인덱스부터 큰 인덱스까지 잘라내도록 보정합니다.
이제 전체 라이다 데이터 중에서 필요한 구간만 가져옵니다.
sector_ranges = msg.ranges[start_index:end_index + 1]
예를 들어 정면 -10도 ~ +10도에 해당하는 인덱스가 170 ~ 190이라면 다음처럼 됩니다.
sector_ranges = msg.ranges[170:191]
Python 슬라이싱에서 끝 인덱스는 포함되지 않습니다.
그래서 end_index + 1을 사용합니다.
msg.ranges[start_index:end_index + 1]
이렇게 해야 end_index 위치의 데이터까지 포함됩니다.
valid_ranges = []
for distance in sector_ranges:
if math.isfinite(distance):
if msg.range_min <= distance <= msg.range_max:
valid_ranges.append(distance)
라이다 데이터에는 항상 정상적인 숫자만 들어오는 것이 아닙니다.
다음과 같은 값이 들어올 수 있습니다.
inf
nan
0.0
라이다 측정 범위보다 작은 값
라이다 측정 범위보다 큰 값
그래서 바로 min()을 사용하면 안 됩니다.
예를 들어 nan이 섞여 있으면 비교가 이상해질 수 있고, inf만 보고 잘못된 판단을 할 수도 있습니다.
먼저 이 코드로 정상적인 숫자인지 확인합니다.
if math.isfinite(distance):
math.isfinite()는 값이 정상적인 유한 숫자인지 확인합니다.
1.2 → True
0.5 → True
inf → False
nan → False
그다음 라이다의 측정 가능 범위 안에 있는지 확인합니다.
if msg.range_min <= distance <= msg.range_max:
예를 들어 라이다 측정 범위가 다음과 같다면,
range_min = 0.12
range_max = 3.5
다음 값만 유효합니다.
0.12m 이상, 3.5m 이하
정상적인 값만 valid_ranges 리스트에 저장합니다.
검사한 구간 안에 유효한 거리값이 하나도 없을 수도 있습니다.
if len(valid_ranges) == 0:
return msg.range_max
검사한 구간 안에 유효한 거리값이 하나도 없을 수도 있습니다.
예를 들어 정면 방향에 감지된 물체가 없거나, 모든 값이 inf, nan일 수 있습니다.
이때 함수가 아무 값도 반환하지 않으면 제어 코드가 망가집니다.
그래서 기본값으로 msg.range_max를 반환합니다.
이 뜻은 다음과 같습니다.
해당 방향에 장애물이 없는 것으로 판단한다.
가장 먼 거리로 처리한다.
즉, 로봇이 “그 방향은 비어 있다”고 해석하도록 만드는 것입니다.
마지막으로 유효한 거리값 중에서 가장 작은 값을 반환합니다.
return min(valid_ranges)
예를 들어 정면 구간의 거리값이 다음과 같다고 해보겠습니다.
valid_ranges = [1.2, 1.1, 0.8, 1.5, 1.3]
이때 반환값은 다음입니다.
0.8
벽 따라가기나 장애물 회피에서는 가장 가까운 장애물이 제일 중요합니다.
정면 구간 안에 대부분은 멀어도 한 지점만 가까우면 충돌할 수 있습니다.
그래서 안전을 위해 최소 거리값을 사용합니다.
6) setup.py에 실행 파일 등록
my_first_package의 setup.py를 엽니다. entry_points 부분에 wall_follower_node를 추가합니다.
entry_points={
'console_scripts': [
'wall_follower_node = my_first_package.wall_follower_node:main',
],
},
전체 구조는 대략 다음처럼 됩니다.
from setuptools import setup
package_name = 'my_first_package'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='user',
maintainer_email='user@example.com',
description='ROS 2 Python practice package',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'wall_follower_node = my_first_package.wall_follower_node:main',
],
},
)
7). package.xml 의존성 확인
package.xml에 다음 의존성이 들어 있어야 합니다.
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
예시는 다음과 같습니다.
<?xml version="1.0"?>
<package format="3">
<name>my_first_package</name>
<version>0.0.0</version>
<description>ROS 2 Python practice package</description>
<maintainer email="user@example.com">user</maintainer>
<license>Apache-2.0</license>
<depend>rclpy</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
Python 패키지이므로 <build_type>은 ament_python입니다.
8) 빌드
워크스페이스 루트로 이동해서 빌드합니다.
cd ~/ros2_study
colcon build --packages-select my_first_package
source install/setup.bash
실행 파일이 등록되었는지 확인합니다.
ros2 run my_first_package wall_follower_node
Gazebo가 아직 켜져 있지 않다면 /scan이 없기 때문에 로봇은 움직이지 않습니다.
실습은 Gazebo와 Cartographer를 먼저 실행한 뒤 진행합니다.
9) 전체 실행
터미널을 3개 사용합니다.
터미널 1: Gazebo 실행
source /opt/ros/humble/setup.bash
source ~/ros2_study/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
터미널 2: Cartographer 실행
source /opt/ros/humble/setup.bash
source ~/ros2_study/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True
터미널 3: Python wall follower 실행
source /opt/ros/humble/setup.bash
source ~/ros2_study/install/setup.bash
ros2 run my_first_package wall_follower_node
실행하면 TurtleBot3가 벽을 따라 움직이기 시작합니다.
RViz에서는 Cartographer가 로봇 이동 경로를 따라 지도를 확장하는 것을 볼 수 있습니다.
10) 동작 확인용 명령어
현재 /cmd_vel 명령 확인:
ros2 topic echo /cmd_vel
라이다 데이터 확인:
ros2 topic echo /scan
노드 목록 확인:
ros2 node list
토픽 연결 관계 확인:
rqt_graph
Cartographer가 정상적으로 실행 중이면 /map, /submap_list, /trajectory_node_list 같은 토픽도 확인할 수 있습니다.
ros2 topic list | grep map
11) my_first_package_msgs를 활용한 상태 메시지 추가
이미 my_first_package_msgs 인터페이스 패키지를 사용하고 있다면 wall following 상태를 커스텀 메시지로 발행해 볼 수 있습니다.
예를 들어 다음 메시지를 만들 수 있습니다.
cd ~/ros2_study/src/my_first_package_msgs
mkdir -p msg
touch msg/WallFollowStatus.msg
WallFollowStatus.msg:
string state
float32 front_distance
float32 right_distance
float32 target_distance
my_first_package_msgs/package.xml에는 다음 의존성이 필요합니다.
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
CMakeLists.txt에는 다음 내용을 넣습니다.
cmake_minimum_required(VERSION 3.8)
project(my_first_package_msgs)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/WallFollowStatus.msg"
)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
빌드합니다.
cd ~/ros2_study
colcon build --packages-select my_first_package_msgs
source install/setup.bash
그다음 my_first_package/package.xml에 커스텀 메시지 패키지 의존성을 추가합니다.
<depend>my_first_package_msgs</depend>
12) 상태 메시지까지 발행하는 wall follower 코드
이번에는 /wall_follow_status 토픽도 함께 발행합니다.
#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from my_first_package_msgs.msg import WallFollowStatus
class WallFollowerNode(Node):
def __init__(self):
super().__init__('wall_follower_node')
self.scan_sub = self.create_subscription(
LaserScan,
'/scan',
self.scan_callback,
qos_profile_sensor_data
)
self.cmd_pub = self.create_publisher(
Twist,
'/cmd_vel',
10
)
self.status_pub = self.create_publisher(
WallFollowStatus,
'/wall_follow_status',
10
)
self.target_wall_distance = 0.45
self.front_safe_distance = 0.55
self.forward_speed = 0.12
self.turn_speed = 0.45
self.get_logger().info('wall_follower_node with status publisher started')
def scan_callback(self, msg: LaserScan):
front = self.get_sector_min_distance(msg, -10.0, 10.0)
right = self.get_sector_min_distance(msg, -100.0, -80.0)
cmd = Twist()
if front < self.front_safe_distance:
cmd.linear.x = 0.0
cmd.angular.z = self.turn_speed
state = 'TURN_LEFT_FRONT_OBSTACLE'
elif right > self.target_wall_distance + 0.12:
cmd.linear.x = self.forward_speed
cmd.angular.z = -0.25
state = 'TURN_RIGHT_FIND_WALL'
elif right < self.target_wall_distance - 0.12:
cmd.linear.x = self.forward_speed * 0.8
cmd.angular.z = 0.25
state = 'TURN_LEFT_TOO_CLOSE'
else:
cmd.linear.x = self.forward_speed
cmd.angular.z = 0.0
state = 'GO_STRAIGHT'
self.cmd_pub.publish(cmd)
status_msg = WallFollowStatus()
status_msg.state = state
status_msg.front_distance = float(front)
status_msg.right_distance = float(right)
status_msg.target_distance = float(self.target_wall_distance)
self.status_pub.publish(status_msg)
self.get_logger().info(
f'state={state}, front={front:.2f}, right={right:.2f}'
)
def get_sector_min_distance(self, msg: LaserScan, start_deg: float, end_deg: float) -> float:
start_rad = math.radians(start_deg)
end_rad = math.radians(end_deg)
start_index = int((start_rad - msg.angle_min) / msg.angle_increment)
end_index = int((end_rad - msg.angle_min) / msg.angle_increment)
start_index = max(0, min(start_index, len(msg.ranges) - 1))
end_index = max(0, min(end_index, len(msg.ranges) - 1))
if start_index > end_index:
start_index, end_index = end_index, start_index
sector_ranges = msg.ranges[start_index:end_index + 1]
valid_ranges = []
for distance in sector_ranges:
if math.isfinite(distance):
if msg.range_min <= distance <= msg.range_max:
valid_ranges.append(distance)
if len(valid_ranges) == 0:
return msg.range_max
return min(valid_ranges)
def main(args=None):
rclpy.init(args=args)
node = WallFollowerNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
stop_cmd = Twist()
node.cmd_pub.publish(stop_cmd)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
빌드 순서는 인터페이스 패키지를 먼저 포함해서 전체 빌드하는 것이 편합니다.
cd ~/ros2_study
colcon build --packages-select my_first_package_msgs my_first_package
source install/setup.bash
상태 토픽 확인:
ros2 topic echo /wall_follow_status
출력 예시는 다음과 비슷합니다.
state: GO_STRAIGHT
front_distance: 1.82
right_distance: 0.46
target_distance: 0.45
13) 파라미터 방식으로 개선하기
현재 코드는 거리값과 속도값이 코드 안에 고정되어 있습니다.
self.target_wall_distance = 0.45
self.front_safe_distance = 0.55
self.forward_speed = 0.12
self.turn_speed = 0.45
위의 변수들은 ROS 2 parameter로 바꾸는 것이 좋습니다.
self.declare_parameter('target_wall_distance', 0.45)
self.declare_parameter('front_safe_distance', 0.55)
self.declare_parameter('forward_speed', 0.12)
self.declare_parameter('turn_speed', 0.45)
self.target_wall_distance = self.get_parameter(
'target_wall_distance'
).get_parameter_value().double_value
self.front_safe_distance = self.get_parameter(
'front_safe_distance'
).get_parameter_value().double_value
self.forward_speed = self.get_parameter(
'forward_speed'
).get_parameter_value().double_value
self.turn_speed = self.get_parameter(
'turn_speed'
).get_parameter_value().double_value
실행할 때 값을 바꿀 수 있습니다.
ros2 run my_first_package wall_follower_node --ros-args \
-p target_wall_distance:=0.50 \
-p front_safe_distance:=0.60 \
-p forward_speed:=0.10 \
-p turn_speed:=0.40
이렇게 하면 코드를 다시 수정하지 않고 실험값만 바꿀 수 있습니다.
14) 지도 저장하기
Cartographer로 맵이 어느 정도 만들어졌다면 지도를 저장할 수 있습니다.
지도 저장:
ros2 run nav2_map_server map_saver_cli -f ~/ros2_study/turtlebot3_wall_map
저장되면 다음 파일이 생성됩니다.
~/ros2_study/turtlebot3_wall_map.yaml
~/ros2_study/turtlebot3_wall_map.pgm
9. Cartographer 파라미터 설정하기
TurtleBot3에서 Cartographer SLAM 파라미터는 ROS 2 일반 YAML 파일이 아니라 Lua 파일에서 설정합니다.
1) 파라미터 파일 위치 확인
APT로 설치한 경우 보통 다음 위치에 있습니다.
/opt/ros/humble/share/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua
확인 명령:
ros2 pkg prefix turtlebot3_cartographer
예상 출력:
/opt/ros/humble
그러면 설정 파일은 다음 위치에 있습니다.
/opt/ros/humble/share/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua
파일을 직접 확인하려면:
cat /opt/ros/humble/share/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua
또는:
gedit /opt/ros/humble/share/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua
다만 /opt/ros/humble 아래 파일을 직접 수정하는 것은 추천하지 않습니다.
패키지 업데이트 시 수정 내용이 사라질 수 있고, 시스템 패키지를 건드리는 방식이라 관리가 좋지 않습니다.
2) 파림터 수정 권장 방식: ros2_study 안에 설정 파일 복사하기
현재 workspace가 ~/ros2_study이므로, 실습용 설정 파일을 따로 보관하겠습니다.
mkdir -p ~/ros2_study/cartographer_config
원본 Lua 파일을 복사합니다.
cp /opt/ros/humble/share/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua \
~/ros2_study/cartographer_config/turtlebot3_lds_2d_custom.lua
수정용 파일을 엽니다.
gedit ~/ros2_study/cartographer_config/turtlebot3_lds_2d_custom.lua
3). Cartographer 실행 파일도 복사하기
기본 launch 파일은 보통 다음 위치에 있습니다.
/opt/ros/humble/share/turtlebot3_cartographer/launch/cartographer.launch.py
확인:
ls /opt/ros/humble/share/turtlebot3_cartographer/launch
실습용 launch 파일을 복사합니다.
mkdir -p ~/ros2_study/cartographer_launch
cp /opt/ros/humble/share/turtlebot3_cartographer/launch/cartographer.launch.py \
~/ros2_study/cartographer_launch/cartographer_custom.launch.py
파일을 엽니다.
gedit ~/ros2_study/cartographer_launch/cartographer_custom.launch.py
launch 파일 안에서 다음과 비슷한 부분을 찾습니다.
configuration_basename=LaunchConfiguration(
'configuration_basename',
default='turtlebot3_lds_2d.lua'
)
그리고 configuration_directory 또는 configuration_basename 관련 부분을 찾아서 수정해야 합니다.
일반적으로 Cartographer launch는 다음 구조를 사용합니다.
configuration_directory
configuration_basename
| 항목 | 의미 |
|---|---|
configuration_directory | Lua 파일이 들어 있는 폴더 |
configuration_basename | 실제 Lua 파일 이름 |
우리가 만든 파일은 다음입니다.
~/ros2_study/cartographer_config/turtlebot3_lds_2d_custom.lua
따라서 launch에서 사용해야 하는 값은 다음과 같습니다.
configuration_directory = ~/ros2_study/cartographer_config
configuration_basename = turtlebot3_lds_2d_custom.lua
단, Python launch 파일 안에서는 ~가 자동으로 풀리지 않을 수 있으므로 절대경로를 쓰는 것이 안전합니다.
예시:
configuration_directory=LaunchConfiguration(
'configuration_directory',
default='/home/사용자이름/ros2_study/cartographer_config'
)
configuration_basename=LaunchConfiguration(
'configuration_basename',
default='turtlebot3_lds_2d_custom.lua'
)
사용자이름은 본인 Ubuntu 계정명으로 바꿉니다.
확인:
whoami
예를 들어 계정명이 robot이면:
default='/home/robot/ros2_study/cartographer_config'
10. TurtleBot3 Cartographer 주요 파라미터
아래 파라미터들은 turtlebot3_lds_2d_custom.lua 안에서 수정합니다.
1) 2D SLAM 사용 설정
MAP_BUILDER.use_trajectory_builder_2d =true
의미:
Cartographer를 2D SLAM 모드로 사용한다.
TurtleBot3 Burger의 기본 LiDAR SLAM에서는 이 값을 true로 둡니다.
2) LiDAR 최소 거리
TRAJECTORY_BUILDER_2D.min_range = 0.12
의미:
이 거리보다 가까운 LiDAR 값은 사용하지 않는다.
예를 들어 너무 가까운 물체나 센서 노이즈 때문에 지도가 지저분하면 값을 조금 키울 수 있습니다.
예시:
TRAJECTORY_BUILDER_2D.min_range =0.15
주의할 점:
너무 크게 설정하면 가까운 벽이나 장애물을 무시할 수 있다.
3) LiDAR 최대 거리
TRAJECTORY_BUILDER_2D.max_range =3.5
의미:
이 거리보다 먼 LiDAR 값은 SLAM 계산에 사용하지 않는다.
TurtleBot3 Burger의 LDS 센서는 짧은 거리 실습에 적합합니다.
시뮬레이션 월드가 작다면 너무 큰 max range가 필요 없습니다.
예시:
TRAJECTORY_BUILDER_2D.max_range =3.0
또는 넓은 공간을 보고 싶다면:
TRAJECTORY_BUILDER_2D.max_range =4.0
주의:
max_range를 무조건 크게 한다고 지도가 좋아지는 것은 아니다.
노이즈까지 같이 들어오면 오히려 지도 품질이 나빠질 수 있다.
4) 먼 거리 데이터 처리 길이
TRAJECTORY_BUILDER_2D.missing_data_ray_length =3.0
의미:
max_range보다 먼 값이 들어왔을 때 어느 정도 길이의 ray로 처리할지 정한다.
쉽게 말하면 LiDAR가 “너무 멀어서 정확히 모르겠다”고 판단한 방향을 어느 정도까지 빈 공간처럼 볼 것인지와 관련됩니다.
5) IMU 사용 여부
TRAJECTORY_BUILDER_2D.use_imu_data =false
의미:
2D SLAM에서 IMU 데이터를 사용할지 결정한다.
TurtleBot3 Gazebo 기본 Cartographer 실습에서는 보통 false로 둡니다.
ROBOTIS 문서도 2D SLAM에서는 range data를 실시간 처리할 수 있으므로 IMU 사용 여부를 선택할 수 있다고 설명합니다.
6) Online Correlative Scan Matching
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =true
의미:
현재 LiDAR 스캔과 기존 submap을 더 적극적으로 맞춰 본다.
장점:
odometry가 조금 부정확해도 scan matching으로 보정 가능
지도 정합성이 좋아질 수 있음
단점:
계산량 증가
CPU 사용량 증가
강의용 TurtleBot3 시뮬레이션에서는 보통 true로 두는 것이 이해하기 좋습니다.
만약 컴퓨터가 느리거나 SLAM이 버벅이면 다음처럼 바꿔 테스트할 수 있습니다.
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =false
7) Motion Filter 각도 기준
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(1.0)
의미:
로봇 회전 변화가 이 값보다 작으면 새 scan을 지도에 넣지 않을 수 있다.
Cartographer는 모든 LaserScan을 무조건 다 지도에 넣지 않습니다.
너무 비슷한 scan을 계속 넣으면 계산량이 커지고 지도가 지저분해질 수 있기 때문입니다.
값을 작게 하면:
더 자주 scan을 반영한다.
지도 반응이 세밀해질 수 있다.
계산량이 증가한다.
값을 크게 하면:
scan을 덜 자주 반영한다.
계산량은 줄어든다.
세밀함은 떨어질 수 있다.
예시:
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(0.5)
또는:
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(2.0)
8) Pose Graph 최적화 주기
POSE_GRAPH.optimize_every_n_nodes =35
의미:
몇 개의 scan node마다 전체 pose graph 최적화를 수행할지 정한다.
쉽게 말하면 Cartographer가 전체 지도를 다시 맞춰 보는 주기입니다.
값을 작게 하면:
더 자주 최적화한다.
loop closure 반응이 빨라질 수 있다.
CPU 사용량이 증가한다.
값을 크게 하면:
최적화를 덜 자주 한다.
CPU 부담은 줄어든다.
지도 보정 반응은 늦어질 수 있다.
예시:
POSE_GRAPH.optimize_every_n_nodes =20
또는:
POSE_GRAPH.optimize_every_n_nodes =60
권장:
POSE_GRAPH.optimize_every_n_nodes =35
9) Constraint Builder 최소 점수
POSE_GRAPH.constraint_builder.min_score =0.65
의미:
scan과 map이 충분히 비슷하다고 판단하는 최소 점수
값을 낮추면:
loop closure 후보를 더 많이 인정한다.
잘못된 매칭 위험이 커진다.
값을 높이면:
확실한 매칭만 인정한다.
loop closure가 덜 일어날 수 있다.
예시:
POSE_GRAPH.constraint_builder.min_score =0.60
또는:
POSE_GRAPH.constraint_builder.min_score =0.70
강의용으로는 기본값을 먼저 사용하고, 지도가 어긋나는 경우에만 조금씩 바꾸는 것이 좋습니다.
10) Global Localization 최소 점수
POSE_GRAPH.constraint_builder.global_localization_min_score =0.7
의미:
전역 위치 후보를 신뢰할 최소 점수
이 값도 loop closure와 관련이 있습니다.
값이 낮으면 더 쉽게 매칭한다.
값이 높으면 더 엄격하게 매칭한다.
너무 낮추면 잘못된 loop closure가 생길 수 있다.
11. 파라미터 실험
처음부터 많은 값을 바꾸면 원인을 알기 어렵습니다.
1) LiDAR 최대 거리 변경
파일:
~/ros2_study/cartographer_config/turtlebot3_lds_2d_custom.lua
기존:
TRAJECTORY_BUILDER_2D.max_range =3.5
변경:
TRAJECTORY_BUILDER_2D.max_range =2.0
관찰할 것:
먼 벽이 지도에 늦게 나타나는가?
지도 범위가 줄어드는가?
가까운 장애물 중심으로 지도 작성이 되는가?
다시 변경:
TRAJECTORY_BUILDER_2D.max_range =4.0
관찰할 것:
먼 벽이 더 잘 보이는가?
지도 노이즈가 늘어나는가?
2) Scan Matching 켜고 끄기
기존:
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =true
변경:
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =false
관찰할 것:
로봇 회전 시 지도가 더 흔들리는가?
CPU 사용량이 줄어드는가?
벽 정합성이 달라지는가?
다시 원복:
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching =true
3) Pose Graph 최적화 주기 변경
기존:
POSE_GRAPH.optimize_every_n_nodes =35
변경:
POSE_GRAPH.optimize_every_n_nodes =10
관찰할 것:
지도 보정이 더 자주 일어나는가?
CPU 사용량이 늘어나는가?
RViz에서 지도가 더 자주 움직이는가?
변경:
POSE_GRAPH.optimize_every_n_nodes =80
관찰할 것:
지도 보정이 늦어지는가?
긴 복도를 돌아왔을 때 loop closure 반응이 늦는가?
4) Motion Filter 변경
기존:
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(1.0)
변경:
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(0.5)
관찰할 것:
회전 중 지도가 더 세밀하게 반영되는가?
계산량이 증가하는가?
변경:
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians =math.rad(3.0)
관찰할 것:
지도 갱신이 둔해지는가?
벽 모양이 덜 정밀해지는가?
12. 수정 후 실행 순서
설정 파일을 수정한 뒤에는 Cartographer를 다시 실행해야 합니다.
터미널 1: Gazebo
source /opt/ros/humble/setup.bash
exportTURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
터미널 2: 수정한 Cartographer 설정으로 실행
source /opt/ros/humble/setup.bash
exportTURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_cartographer cartographer.launch.py \
use_sim_time:=True \
configuration_directory:=/home/사용자이름/ros2_study/cartographer_config \
configuration_basename:=turtlebot3_lds_2d_custom.lua
터미널 3: 로봇 이동
키보드 조작:
ros2 run turtlebot3_teleop teleop_keyboard