1. ROS 2 모바일 로봇 전체 흐름
차동구동 로봇에서 ROS 2 제어 흐름은 보통 다음과 같습니다.
사용자 / Teleop / Nav2
↓
/cmd_vel
geometry_msgs/msg/Twist
↓
차동구동 역기구학
↓
왼쪽 바퀴 속도, 오른쪽 바퀴 속도
↓
모터 드라이버 / MCU / OpenCR / ros2_control
↓
바퀴 회전
↓
엔코더 피드백
↓
거리 변환
↓
Odometry 계산
↓
/odom
nav_msgs/msg/Odometry
↓
odom → base_link TF
↓
RViz2 / Nav2 / SLAM / Localization
먼저 아래와 같은 ROS 2 토픽을 이해해야 합니다.
| /cmd_vel | geometry_msgs/msg/Twist | 로봇 속도 명령 |
| /odom | nav_msgs/msg/Odometry | 로봇 위치/자세/속도 추정 |
| /tf | tf2_msgs/msg/TFMessage | 동적 좌표 변환 |
| /tf_static | tf2_msgs/msg/TFMessage | 고정 좌표 변환 |
| /joint_states | sensor_msgs/msg/JointState | 바퀴 조인트 상태 |
| /scan | sensor_msgs/msg/LaserScan | LiDAR 데이터 |
| /imu/data | sensor_msgs/msg/Imu | IMU 데이터 |
2. ROS 좌표계: odom, base_link, map
ROS 2 모바일 로봇에서 가장 기본적인 TF(Transform, 좌표계 변환 시스템) 구조는 다음과 같습니다.
map
└── odom
└── base_link
├── laser
├── imu_link
└── wheel_link
아래의 연결을 이해하는 것이 중요합니다.
odom → base_linkodom
odom은 로봇이 출발한 지점 기준으로 부드럽게 이어지는 로컬 좌표계입니다.
특징:
연속적이다.
갑자기 튀면 안 된다.
시간이 지나면 drift가 생긴다.base_link
base_link는 로봇 본체 중심 좌표계입니다.
ROS 모바일 로봇의 기본 방향은 다음과 같습니다.
x축: 로봇 전방
y축: 로봇 왼쪽
z축: 위쪽map
map은 전역 지도 좌표계입니다. SLAM 또는 AMCL이 map → odom 변환을 보정합니다.
정리하면:
odom → base_link : 엔코더/IMU 기반 로컬 오도메트리
map → odom : SLAM/AMCL 기반 전역 위치 보정
이 구조를 깨면 Nav2가 제대로 동작하지 않습니다.
3. /cmd_vel 메시지 이해
ROS 2에서 모바일 로봇 속도 명령은 보통 /cmd_vel 토픽으로 보냅니다.
메시지 타입은 다음입니다.
ros2 interface show geometry_msgs/msg/Twist
구조는 다음과 같습니다.
Vector3 linear
Vector3 angular
차동구동 로봇에서는 보통 두 값만 씁니다.
linear.x : 전진/후진 속도 [m/s]
angular.z : 좌회전/우회전 각속도 [rad/s]
예를 들어:
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{
linear: {x: 0.2, y: 0.0, z: 0.0},
angular: {x: 0.0, y: 0.0, z: 0.5}
}"
의미는 다음입니다.
전진 속도 v = 0.2 m/s
회전 속도 omega = 0.5 rad/s
즉 로봇은 앞으로 가면서 왼쪽으로 회전합니다.
4. 엔코더 기반 Odometry 유도
이제 엔코더 tick에서 로봇 위치를 계산하는 과정을 보겠습니다.
1) tick을 거리로 변환
왼쪽과 오른쪽 엔코더 누적 tick이 있다고 가정합니다.
$$
left\_ticks,\quad right\_ticks
$$
이전 주기의 tick과 비교해서 변화량을 구합니다.
$$
\Delta left\_ticks = left\_ticks - prev\_left\_ticks
$$
$$
\Delta right\_ticks = right\_ticks - prev\_right\_ticks
$$
tick 하나당 이동 거리는 다음과 같습니다.
$$
meter\_per\_tick = \frac{2\pi r}{ticks\_per\_revolution}
$$
왼쪽, 오른쪽 바퀴 이동 거리는 다음과 같습니다.
$$
d_l = \Delta left\_ticks \times meter\_per\_tick
$$
$$
d_r = \Delta right\_ticks \times meter\_per\_tick
$$
2) 로봇 중심 이동 거리
로봇 중심 이동 거리는 좌우 바퀴 이동 거리의 평균입니다.
$$
d_{center} = \frac{d_r + d_l}{2}
$$
3) 방향 변화량
로봇의 yaw 변화량은 좌우 바퀴 이동 거리 차이를 바퀴 간 거리로 나눈 값입니다.
$$
\Delta \theta = \frac{d_r - d_l}{L}
$$
오른쪽 바퀴가 더 많이 이동하면 다음과 같습니다.
$$
d_r > d_l
$$
$$
\Delta \theta > 0
$$
ROS 좌표계 기준으로 yaw가 증가하므로 반시계 방향, 즉 왼쪽 회전입니다.
4) $x$, $y$, $\theta$ 업데이트
현재 상태가 다음이라고 하겠습니다.
$$
x,\quad y,\quad \theta
$$
한 주기 동안 이동 후 상태는 다음처럼 갱신합니다.
$$
x_{new} = x + d_{center}\cos\left(\theta + \frac{\Delta \theta}{2}\right)
$$
$$
y_{new} = y + d_{center}\sin\left(\theta + \frac{\Delta \theta}{2}\right)
$$
$$
\theta_{new} = \theta + \Delta \theta
$$
여기서 다음 항을 쓰는 이유가 중요합니다.
$$
\theta + \frac{\Delta \theta}{2}
$$
로봇은 한 주기 동안 직선으로만 이동하지 않습니다. 회전하면서 조금씩 곡선을 그립니다. 시작 방향 $\theta$만 쓰면 오차가 커질 수 있습니다. 그래서 이동 중간 방향을 사용합니다.
이 방식은 차동구동 odometry에서 가장 많이 쓰는 기본 적분 방식입니다.
5. 인터페이스 패키지에 EncoderTicks 메시지 추가
먼저 my_first_package_msgs에 다음 메시지를 추가합니다.
파일 경로:
~/ros2_study/src/my_first_package_msgs/msg/EncoderTicks.msg
내용:
int64 left_ticks
int64 right_ticks
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에는 다음을 추가합니다.
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/EncoderTicks.msg"
)
ament_package()
빌드 후에는 다음 타입을 사용할 수 있습니다.
my_first_package_msgs/msg/EncoderTicks
6. /cmd_vel을 좌우 바퀴 속도로 변환하는 Python 노드
/cmd_vel은 로봇 중심 기준 속도 명령입니다.
하지만 실제 모터 드라이버는 왼쪽 바퀴와 오른쪽 바퀴 명령을 따로 받아야 합니다.
그래서 첫 번째로 해야 할 일은 cmd_vel을 좌우 바퀴 속도로 변환하는 것입니다.
/cmd_vel
↓
left wheel velocity
right wheel velocity
입력은 로봇의 목표 속도입니다.
$$
\text{입력} = v,\ \omega
$$
출력은 좌우 바퀴 속도입니다.
$$
\text{출력} = v_l,\ v_r
$$
차동 로봇의 기본 역기구학 식은 다음과 같습니다.
$$
v_r = v + \frac{\omega L}{2}
$$
$$
v_l = v - \frac{\omega L}{2}
$$
바퀴 선속도가 아니라 모터 각속도가 필요하다면 다음처럼 바꿉니다.
$$
\omega_r = \frac{v_r}{r}
$$
$$
\omega_l = \frac{v_l}{r}
$$
따라서 다음과 같이 정리할 수 있습니다.
$$
\omega_r = \frac{v + \frac{\omega L}{2}}{r}
$$
$$
\omega_l = \frac{v - \frac{\omega L}{2}}{r}
$$
파일 경로:
~/ros2_study/src/my_first_package/my_first_package/cmd_vel_to_wheel.py
소스 코드:
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64MultiArray
class CmdVelToWheel(Node):
def __init__(self):
super().__init__('cmd_vel_to_wheel')
self.declare_parameter('wheel_radius', 0.033)
self.declare_parameter('wheel_separation', 0.30)
self.wheel_radius = self.get_parameter('wheel_radius').value
self.wheel_separation = self.get_parameter('wheel_separation').value
self.sub_cmd_vel = self.create_subscription(
Twist,
'/cmd_vel',
self.cmd_vel_callback,
10
)
self.pub_wheel_velocity = self.create_publisher(
Float64MultiArray,
'/wheel_velocity',
10
)
self.get_logger().info('cmd_vel_to_wheel node started')
self.get_logger().info(f'wheel_radius: {self.wheel_radius} m')
self.get_logger().info(f'wheel_separation: {self.wheel_separation} m')
def cmd_vel_callback(self, msg: Twist):
linear_x = msg.linear.x
angular_z = msg.angular.z
left_linear_velocity = (
linear_x - angular_z * self.wheel_separation / 2.0
)
right_linear_velocity = (
linear_x + angular_z * self.wheel_separation / 2.0
)
left_angular_velocity = left_linear_velocity / self.wheel_radius
right_angular_velocity = right_linear_velocity / self.wheel_radius
out = Float64MultiArray()
out.data = [
left_linear_velocity,
right_linear_velocity,
left_angular_velocity,
right_angular_velocity
]
self.pub_wheel_velocity.publish(out)
self.get_logger().info(
'cmd_vel '
f'linear.x={linear_x:.3f} m/s, '
f'angular.z={angular_z:.3f} rad/s | '
f'left={left_linear_velocity:.3f} m/s, '
f'right={right_linear_velocity:.3f} m/s | '
f'left_w={left_angular_velocity:.3f} rad/s, '
f'right_w={right_angular_velocity:.3f} rad/s'
)
def main(args=None):
rclpy.init(args=args)
node = CmdVelToWheel()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
/cmd_vel에서 받은 linear.x는 로봇의 전진 속도이고, angular.z는 회전 속도입니다. 이 값을 이용해 좌우 바퀴의 선속도를 계산합니다.
left_linear_velocity = linear_x - angular_z * wheel_separation / 2.0
right_linear_velocity = linear_x + angular_z * wheel_separation / 2.0
이후 각 바퀴의 선속도를 바퀴 반지름으로 나누어 바퀴의 각속도(rad/s)로 변환합니다.
left_angular_velocity = left_linear_velocity / wheel_radius
right_angular_velocity = right_linear_velocity / wheel_radius
최종적으로 /wheel_velocity 토픽에는 다음 순서로 데이터가 발행됩니다.
[
left_linear_velocity,
right_linear_velocity,
left_angular_velocity,
right_angular_velocity
]
즉, 이 노드는 상위 제어기나 키보드 조종 노드에서 생성한 /cmd_vel 명령을 실제 좌우 바퀴 제어에 사용할 수 있는 속도 값으로 변환하는 역할을 합니다. 차동구동 모바일 로봇의 모터 제어 전단에서 사용하기 적합한 기본 변환 노드입니다.
setup.py의 entry_points에 추가합니다.
entry_points={
'console_scripts': [
'cmd_vel_to_wheel = my_first_package.cmd_vel_to_wheel:main',
],
},
예를 들어 다음 명령이 들어왔다고 가정합니다.
linear.x = 0.3 m/s
angular.z = 0.5 rad/s
wheel_radius = 0.033 m
wheel_separation = 0.30 m
계산 결과는 다음과 비슷합니다.
left_linear_velocity ≈ 0.225 m/s
right_linear_velocity ≈ 0.375 m/s
left_angular_velocity ≈ 6.818 rad/s
right_angular_velocity ≈ 11.364 rad/s
오른쪽 바퀴가 더 빠르므로 로봇은 왼쪽으로 회전합니다.
7. 가상 Encoder Tick 발행 노드 만들기
실제 로봇에서는 엔코더 tick이 MCU, 모터 드라이버, ros2_control hardware interface 등을 통해 들어옵니다.
먼저 가상 엔코더 tick을 발행해서 odometry 원리를 이해하려고 합니다.
fake_encoder_node
↓
/encoder_ticks
파일 경로:
~/ros2_study/src/my_first_package/my_first_package/fake_encoder_node.py
코드:
import rclpy
from rclpy.node import Node
from my_first_package_msgs.msg import EncoderTicks
class FakeEncoderNode(Node):
def __init__(self):
super().__init__('fake_encoder_node')
self.publisher = self.create_publisher(
EncoderTicks,
'/encoder_ticks',
10
)
self.left_ticks = 0
self.right_ticks = 0
self.declare_parameter('left_tick_step', 5)
self.declare_parameter('right_tick_step', 5)
self.declare_parameter('period', 0.05)
self.left_tick_step = self.get_parameter('left_tick_step').value
self.right_tick_step = self.get_parameter('right_tick_step').value
self.period = self.get_parameter('period').value
self.timer = self.create_timer(self.period, self.timer_callback)
self.get_logger().info('fake_encoder_node started')
def timer_callback(self):
self.left_ticks += int(self.left_tick_step)
self.right_ticks += int(self.right_tick_step)
msg = EncoderTicks()
msg.left_ticks = self.left_ticks
msg.right_ticks = self.right_ticks
self.publisher.publish(msg)
self.get_logger().info(
f'left_ticks={self.left_ticks}, '
f'right_ticks={self.right_ticks}'
)
def main(args=None):
rclpy.init(args=args)
node = FakeEncoderNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
노드가 실행되면 내부 변수 left_ticks, right_ticks가 0에서 시작합니다. 이후 타이머가 period 주기마다 실행되며, 각 tick 값에 설정된 증가량을 더합니다.
self.left_ticks += int(self.left_tick_step)
self.right_ticks += int(self.right_tick_step)
증가된 엔코더 값은 EncoderTicks 메시지에 담겨 /encoder_ticks 토픽으로 발행됩니다.
msg.left_ticks = self.left_ticks
msg.right_ticks = self.right_ticks
즉, 이 노드는 실제 엔코더 없이도 엔코더 데이터가 계속 들어오는 것처럼 테스트할 수 있게 해주는 가상 엔코더 발행 노드입니다. 주로 오도메트리 계산 노드, 속도 계산 노드, 바퀴 제어 알고리즘을 검증할 때 사용할 수 있습니다.
setup.py에 추가합니다.
entry_points={
'console_scripts': [
'cmd_vel_to_wheel = my_first_package.cmd_vel_to_wheel:main',
'fake_encoder_node = my_first_package.fake_encoder_node:main',
],
},
기본값은 왼쪽과 오른쪽 tick이 똑같이 증가합니다.
left_ticks += 5
right_ticks += 5
이 경우 로봇은 직진하는 상황입니다.
오른쪽을 더 빠르게 만들면 다음과 같습니다.
left_ticks += 3
right_ticks += 7
이 경우 오른쪽 바퀴 이동량이 더 크므로 로봇은 왼쪽으로 회전합니다.
제자리 회전은 다음과 같습니다.
left_ticks -= 5
right_ticks += 5
이 경우 중심 이동거리는 거의 0이고 회전각만 증가합니다.
8. Encoder Tick을 거리로 변환하는 Python 노드
엔코더 tick은 그 자체로는 거리 단위가 아닙니다.
바퀴 반지름과 바퀴 1회전당 tick 수를 알아야 실제 이동거리로 바꿀 수 있습니다.
바퀴 둘레는 다음 식으로 표현됩니다.
wheel_circumference = 2πr
tick 하나당 이동거리는 다음입니다.
meter_per_tick = 2πr / ticks_per_revolution
왼쪽, 오른쪽 바퀴 이동거리는 다음입니다.
d_left = Δleft_ticks × meter_per_tick
d_right = Δright_ticks × meter_per_tick
파일 경로:
~/ros2_study/src/my_first_package/my_first_package/tick_to_distance.py
코드:
import math
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from my_first_package_msgs.msg import EncoderTicks
class TickToDistance(Node):
def __init__(self):
super().__init__('tick_to_distance')
self.declare_parameter('wheel_radius', 0.033)
self.declare_parameter('ticks_per_revolution', 2048)
self.wheel_radius = self.get_parameter('wheel_radius').value
self.ticks_per_revolution = self.get_parameter(
'ticks_per_revolution'
).value
self.distance_per_tick = (
2.0 * math.pi * self.wheel_radius
/ float(self.ticks_per_revolution)
)
self.prev_left_ticks = None
self.prev_right_ticks = None
self.sub_encoder = self.create_subscription(
EncoderTicks,
'/encoder_ticks',
self.encoder_callback,
10
)
self.pub_wheel_distance = self.create_publisher(
Float64MultiArray,
'/wheel_distance',
10
)
self.get_logger().info('tick_to_distance node started')
self.get_logger().info(f'wheel_radius: {self.wheel_radius} m')
self.get_logger().info(
f'ticks_per_revolution: {self.ticks_per_revolution}'
)
self.get_logger().info(
f'distance_per_tick: {self.distance_per_tick:.8f} m/tick'
)
def encoder_callback(self, msg: EncoderTicks):
left_ticks = msg.left_ticks
right_ticks = msg.right_ticks
if self.prev_left_ticks is None:
self.prev_left_ticks = left_ticks
self.prev_right_ticks = right_ticks
self.get_logger().info('First encoder data received')
return
delta_left_ticks = left_ticks - self.prev_left_ticks
delta_right_ticks = right_ticks - self.prev_right_ticks
self.prev_left_ticks = left_ticks
self.prev_right_ticks = right_ticks
left_distance = delta_left_ticks * self.distance_per_tick
right_distance = delta_right_ticks * self.distance_per_tick
out = Float64MultiArray()
out.data = [
float(delta_left_ticks),
float(delta_right_ticks),
left_distance,
right_distance
]
self.pub_wheel_distance.publish(out)
self.get_logger().info(
f'delta_ticks L={delta_left_ticks}, R={delta_right_ticks} | '
f'distance L={left_distance:.5f} m, '
f'R={right_distance:.5f} m'
)
def main(args=None):
rclpy.init(args=args)
node = TickToDistance()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
먼저 바퀴 한 바퀴의 둘레를 엔코더 해상도로 나누어 tick 1개당 이동 거리를 계산합니다.
distance_per_tick = 2.0 * math.pi * wheel_radius / ticks_per_revolution
엔코더 값은 누적 tick 값으로 들어오기 때문에, 현재 tick 값에서 이전 tick 값을 빼서 변화량을 구합니다.
delta_left_ticks = left_ticks - self.prev_left_ticks
delta_right_ticks = right_ticks - self.prev_right_ticks
이후 tick 변화량에 distance_per_tick을 곱해 좌우 바퀴의 이동 거리를 계산합니다.
left_distance = delta_left_ticks * self.distance_per_tick
right_distance = delta_right_ticks * self.distance_per_tick
최종적으로 /wheel_distance 토픽에는 다음 순서로 데이터가 발행됩니다.
[
delta_left_ticks,
delta_right_ticks,
left_distance,
right_distance
]
첫 번째 엔코더 데이터는 이전 값이 없기 때문에 기준값으로만 저장하고 거리 계산은 하지 않습니다. 두 번째 데이터부터 실제 tick 변화량과 이동 거리를 계산합니다.
즉, 이 노드는 엔코더의 누적 tick 값을 실제 바퀴 이동 거리로 변환하는 역할을 합니다. 차동구동 로봇에서 오도메트리 계산, 주행 거리 추정, 속도 계산 노드의 전단에 사용하기 적합합니다.
setup.py에 추가합니다.
entry_points={
'console_scripts': [
'cmd_vel_to_wheel = my_first_package.cmd_vel_to_wheel:main',
'fake_encoder_node = my_first_package.fake_encoder_node:main',
'tick_to_distance = my_first_package.tick_to_distance:main',
],
},
예를 들어 다음 조건이라고 가정합니다.
wheel_radius = 0.033 m
ticks_per_revolution = 2048
delta_ticks = 100
계산 결과는 다음과 같습니다.
distance_per_tick ≈ 0.0001012 m/tick
100 ticks ≈ 0.01012 m
좌우 이동거리가 같으면 직진입니다.
오른쪽 이동거리가 더 크면 로봇은 왼쪽으로 회전한 것으로 해석합니다.
9. Python으로 Encoder Odometry 노드 구현하기
이번에는 my_first_package에 Python 노드를 만들고, /encoder_ticks를 받아서 /odom과 /tf를 발행합니다.
/encoder_ticks
↓
encoder_odom_node
↓
/odom
/tf : odom → base_link
파일 경로 예시는 다음과 같습니다.
~/ros2_study/src/my_first_package/my_first_package/encoder_odom_node.py1) Python 코드
import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
from my_first_package_msgs.msg import EncoderTicks
class EncoderOdomNode(Node):
def __init__(self):
super().__init__('encoder_odom_node')
self.declare_parameter('wheel_radius', 0.033)
self.declare_parameter('wheel_separation', 0.30)
self.declare_parameter('ticks_per_revolution', 2048)
self.declare_parameter('odom_frame_id', 'odom')
self.declare_parameter('base_frame_id', 'base_link')
self.declare_parameter('publish_tf', True)
self.wheel_radius = self.get_parameter('wheel_radius').value
self.wheel_separation = self.get_parameter('wheel_separation').value
self.ticks_per_revolution = self.get_parameter(
'ticks_per_revolution'
).value
self.odom_frame_id = self.get_parameter('odom_frame_id').value
self.base_frame_id = self.get_parameter('base_frame_id').value
self.publish_tf = self.get_parameter('publish_tf').value
self.distance_per_tick = (
2.0 * math.pi * self.wheel_radius
/ float(self.ticks_per_revolution)
)
self.prev_left_ticks = 0
self.prev_right_ticks = 0
self.prev_time = None
self.has_prev_ticks = False
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.odom_pub = self.create_publisher(
Odometry,
'/odom',
10
)
self.tf_broadcaster = TransformBroadcaster(self)
self.encoder_sub = self.create_subscription(
EncoderTicks,
'/encoder_ticks',
self.encoder_callback,
10
)
self.get_logger().info('encoder_odom_node started')
self.get_logger().info(f'wheel_radius: {self.wheel_radius:.4f} m')
self.get_logger().info(
f'wheel_separation: {self.wheel_separation:.4f} m'
)
self.get_logger().info(
f'ticks_per_revolution: {self.ticks_per_revolution}'
)
self.get_logger().info(
f'distance_per_tick: {self.distance_per_tick:.8f} m/tick'
)
def encoder_callback(self, msg: EncoderTicks):
now = self.get_clock().now()
left_ticks = msg.left_ticks
right_ticks = msg.right_ticks
if not self.has_prev_ticks:
self.prev_left_ticks = left_ticks
self.prev_right_ticks = right_ticks
self.prev_time = now
self.has_prev_ticks = True
self.get_logger().info('First encoder data received')
return
dt = (now - self.prev_time).nanoseconds / 1e9
if dt <= 0.0:
return
delta_left_ticks = left_ticks - self.prev_left_ticks
delta_right_ticks = right_ticks - self.prev_right_ticks
self.prev_left_ticks = left_ticks
self.prev_right_ticks = right_ticks
self.prev_time = now
d_left = float(delta_left_ticks) * self.distance_per_tick
d_right = float(delta_right_ticks) * self.distance_per_tick
d_center = (d_right + d_left) * 0.5
d_theta = (d_right - d_left) / self.wheel_separation
heading_mid = self.theta + d_theta * 0.5
self.x += d_center * math.cos(heading_mid)
self.y += d_center * math.sin(heading_mid)
self.theta += d_theta
self.theta = math.atan2(
math.sin(self.theta),
math.cos(self.theta)
)
linear_velocity = d_center / dt
angular_velocity = d_theta / dt
qx = 0.0
qy = 0.0
qz = math.sin(self.theta * 0.5)
qw = math.cos(self.theta * 0.5)
self.publish_odometry(
now,
qx,
qy,
qz,
qw,
linear_velocity,
angular_velocity
)
if self.publish_tf:
self.publish_transform(
now,
qx,
qy,
qz,
qw
)
self.get_logger().info(
f'x={self.x:.3f}, y={self.y:.3f}, '
f'theta={self.theta:.3f} | '
f'v={linear_velocity:.3f} m/s, '
f'w={angular_velocity:.3f} rad/s'
)
def publish_odometry(
self,
now,
qx,
qy,
qz,
qw,
linear_velocity,
angular_velocity
):
odom_msg = Odometry()
odom_msg.header.stamp = now.to_msg()
odom_msg.header.frame_id = self.odom_frame_id
odom_msg.child_frame_id = self.base_frame_id
odom_msg.pose.pose.position.x = self.x
odom_msg.pose.pose.position.y = self.y
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation.x = qx
odom_msg.pose.pose.orientation.y = qy
odom_msg.pose.pose.orientation.z = qz
odom_msg.pose.pose.orientation.w = qw
odom_msg.twist.twist.linear.x = linear_velocity
odom_msg.twist.twist.linear.y = 0.0
odom_msg.twist.twist.angular.z = angular_velocity
odom_msg.pose.covariance = [
0.001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 999.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 999.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 999.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01
]
odom_msg.twist.covariance = [
0.001, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.001, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 999.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 999.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 999.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01
]
self.odom_pub.publish(odom_msg)
def publish_transform(
self,
now,
qx,
qy,
qz,
qw
):
tf_msg = TransformStamped()
tf_msg.header.stamp = now.to_msg()
tf_msg.header.frame_id = self.odom_frame_id
tf_msg.child_frame_id = self.base_frame_id
tf_msg.transform.translation.x = self.x
tf_msg.transform.translation.y = self.y
tf_msg.transform.translation.z = 0.0
tf_msg.transform.rotation.x = qx
tf_msg.transform.rotation.y = qy
tf_msg.transform.rotation.z = qz
tf_msg.transform.rotation.w = qw
self.tf_broadcaster.sendTransform(tf_msg)
def main(args=None):
rclpy.init(args=args)
node = EncoderOdomNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()2) Python 패키지 설정
C++ 패키지에서는 CMakeLists.txt와 package.xml에 실행 파일과 의존성을 설정했지만, Python 패키지에서는 주로 setup.py, package.xml을 수정합니다.
my_first_package/package.xml에 다음 의존성이 필요합니다.
<depend>rclpy</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>my_first_package_msgs</depend>
Python 패키지라면 export 부분에 아래 내용도 확인합니다.
<export>
<build_type>ament_python</build_type>
</export>3) setup.py 설정
my_first_package/setup.py의 entry_points에 encoder_odom_node를 추가합니다.
entry_points={
'console_scripts': [
'fake_encoder_node = my_first_package.fake_encoder_node:main',
'encoder_odom_node = my_first_package.encoder_odom_node:main',
],
},
launch 파일을 설치하려면 data_files에 launch 경로도 포함합니다.
import os
from glob import glob
그리고 data_files에 다음을 추가합니다.
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
예시는 다음과 같습니다.
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
],4) Odometry 메시지 구조 이해
/odom 토픽은 일반적으로 다음 메시지 타입을 사용합니다.
nav_msgs/msg/Odometry
핵심 필드는 다음과 같습니다.
header.frame_id = "odom"
child_frame_id = "base_link"
pose.pose.position.x
pose.pose.position.y
pose.pose.orientation
twist.twist.linear.x
twist.twist.angular.z
pose.covariance
twist.covariance
pose와 twist는
pose : header.frame_id 기준
twist : child_frame_id 기준
입니다. 즉, 다음처럼 해석합니다.
위치 pose : odom 기준에서 base_link의 위치
속도 twist : base_link 기준에서 로봇의 속도5) Odometry 계산 핵심 공식
엔코더 변화량은 다음처럼 계산합니다.
delta_left_ticks = left_ticks - self.prev_left_ticks
delta_right_ticks = right_ticks - self.prev_right_ticks
tick을 거리로 바꿉니다.
d_left = float(delta_left_ticks) * self.distance_per_tick
d_right = float(delta_right_ticks) * self.distance_per_tick
로봇 중심 이동거리와 회전량은 다음입니다.
d_center = (d_right + d_left) * 0.5
d_theta = (d_right - d_left) / self.wheel_separation
현재 자세가 x, y, theta일 때 위치 갱신은 다음입니다.
heading_mid = self.theta + d_theta * 0.5
self.x += d_center * math.cos(heading_mid)
self.y += d_center * math.sin(heading_mid)
self.theta += d_theta
heading_mid를 쓰는 이유는 로봇이 이동하면서 동시에 회전하기 때문입니다.
시작 방향이나 끝 방향 하나만 사용하는 것보다 중간 방향으로 적분하는 방식이 더 자연스럽습니다.
각도 theta는 다음 코드로 -pi ~ pi 범위로 정규화합니다.
self.theta = math.atan2(
math.sin(self.theta),
math.cos(self.theta)
)atan2(y, x)는
점 (x, y)가 원점에서 볼 때 몇 도 방향인지를 알려주는 함수입니다.
여기서는
atan2(sin(theta), cos(theta))이므로,
점 (cos(theta), sin(theta))를 보고
그 점이 가리키는 각도를 다시 구한다는 뜻입니다.
즉,
- 원래 각도 theta를
- 원 위의 점 (cos(theta), sin(theta))로 바꾼 뒤
- 그 점의 각도를 다시 구합니다.
그런데 atan2()는 결과를 항상 보통 -π ~ π 범위로 돌려줍니다.
그래서 자동으로 각도가 정리됩니다.
점 P는 30도 방향에 있습니다.
atan2(sin(30°), cos(30°)) = 30°그대로입니다.
390도는 한 바퀴(360도)를 돌고 다시 30도 방향입니다.
390° = 360° + 30°이때도
atan2(sin(390°), cos(390°)) = 30°가 됩니다.
즉,
- 390°라는 “긴 표현”을
- 30°라는 “정리된 표현”으로 바꾸는 것입니다.
190도는 π보다 큰 각도입니다.
원 위에서는 왼쪽 아래쪽 방향입니다.
190도는 사실 방향상으로는 -170°와 같습니다.
왜냐하면:
190° = -170° + 360°즉 같은 방향입니다.
그래서
atan2(sin(190°), cos(190°)) = -170°가 됩니다.
즉 atan2()는 같은 방향을 더 짧고 표준적인 범위로 표현해 줍니다.
로봇에서는 보통 -π ~ π를 많이 씁니다.
이유는 “좌회전/우회전” 해석이 직관적이기 때문입니다.
예를 들어,
- +0.2 rad → 약간 반시계 방향
- -0.2 rad → 약간 시계 방향
처럼 바로 해석하기 편합니다.
또한 각도 차이를 계산할 때도 더 자연스럽습니다.
6) odom → base_link TF 발행
/odom 토픽만 발행하면 위치와 속도 숫자는 확인할 수 있습니다.
하지만 RViz2, Nav2, SLAM, 센서 시각화에서는 TF가 중요합니다.
이번 Python 노드에서는 다음 TF를 발행합니다.
odom → base_link
의미는 다음과 같습니다.
odom 좌표계 기준에서 base_link가 어디에 있는가?
TF 메시지는 다음 타입을 사용합니다.
geometry_msgs/msg/TransformStamped
tf_msg.header.frame_id = self.odom_frame_id
tf_msg.child_frame_id = self.base_frame_id
tf_msg.transform.translation.x = self.x
tf_msg.transform.translation.y = self.y
tf_msg.transform.translation.z = 0.0
2D 로봇에서는 roll, pitch를 사용하지 않고 yaw만 사용합니다.
yaw를 quaternion으로 바꾸는 기본식은 다음과 같습니다.
qx = 0.0
qy = 0.0
qz = math.sin(self.theta * 0.5)
qw = math.cos(self.theta * 0.5)covariance는 센서값이나 계산값의 불확실성을 의미합니다.
값이 작을수록:
이 값은 비교적 믿을 만하다
값이 클수록:
이 값은 신뢰도가 낮다라는 의미입니다.
8) pose.covariance의 의미
odom_msg.pose.covariance = [...]
이 부분은 로봇의 위치와 자세에 대한 신뢰도를 설정합니다.
pose는 다음 6개 값에 대한 covariance를 가집니다.
x, y, z, roll, pitch, yaw즉, 6x6 행렬입니다.
Python 코드에서는 6x6 행렬을 한 줄짜리 리스트 36개 값으로 넣습니다.
[0][0] = x 위치 신뢰도
[1][1] = y 위치 신뢰도
[2][2] = z 위치 신뢰도
[3][3] = roll 신뢰도
[4][4] = pitch 신뢰도
[5][5] = yaw 신뢰도
현재 코드에서는 대각선 값이 핵심입니다.
x = 0.001
y = 0.001
z = 999.0
roll = 999.0
pitch = 999.0
yaw = 0.01
0.001은 비교적 신뢰, 999.0은 거의 사용하지 않음, 0.10은 어느 정도 신뢰를 의미합니다.
차동구동 로봇은 보통 2D 평면에서 움직이므로 x, y, yaw가 중요합니다.
반대로 z, roll, pitch는 거의 사용하지 않기 때문에 큰 값을 넣어 신뢰도가 낮다고 표시합니다.
9) twist.covariance의 의미
odom_msg.twist.covariance = [...]
이 부분은 로봇의 속도에 대한 신뢰도를 설정합니다.
twist도 6개 값에 대한 covariance를 가집니다.
linear.x, linear.y, linear.z, angular.x, angular.y, angular.z
현재 설정은 2D 이동 로봇에서 사용하는 전진 속도와 yaw 회전 속도는 비교적 신뢰하고, 3D 방향 속도나 roll/pitch 관련 속도는 신뢰하지 않는다는 뜻입니다.
linear.x = 0.001
linear.y = 0.001
linear.z = 999.0
angular.x = 999.0
angular.y = 999.0
angular.z = 0.01
10. Python용 Launch 파일 만들기
두 노드를 한 번에 실행하기 위해 launch 파일을 만듭니다.
파일 경로:
~/ros2_study/src/my_first_package/launch/odom_demo.launch.py
내용:
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
fake_encoder_node = Node(
package='my_first_package',
executable='fake_encoder_node',
name='fake_encoder_node',
output='screen',
parameters=[
{
'left_tick_step': 5,
'right_tick_step': 5,
'period': 0.05
}
]
)
encoder_odom_node = Node(
package='my_first_package',
executable='encoder_odom_node',
name='encoder_odom_node',
output='screen',
parameters=[
{
'wheel_radius': 0.033,
'wheel_separation': 0.30,
'ticks_per_revolution': 2048,
'odom_frame_id': 'odom',
'base_frame_id': 'base_link',
'publish_tf': True
}
]
)
return LaunchDescription([
fake_encoder_node,
encoder_odom_node
])
11) 빌드 및 실행
패키지 루트가 포함된 워크스페이스에서 빌드합니다.
cd ~/ros2_study
colcon build --packages-select my_first_package my_first_package_msgs
source install/setup.bash
launch 파일로 실행합니다.
ros2 launch my_first_package odom_demo.launch.py
개별 실행도 가능합니다.
ros2 run my_first_package fake_encoder_node
ros2 run my_first_package encoder_odom_node
토픽 확인:
ros2 topic echo /encoder_ticksros2 topic echo /odom
TF 확인:
ros2 run tf2_ros tf2_echo odom base_link
12. RViz2에서 확인하는 방법
RViz2 설정은 다음처럼 합니다.
Fixed Frame : odom
Add → TF
Add → Odometry
Odometry Topic : /odom
정상이라면 base_link가 odom 기준으로 앞으로 이동합니다.
직진 조건:
left_tick_step = 5
right_tick_step = 5
예상 결과:
x 증가
y 거의 변화 없음
theta 거의 변화 없음
왼쪽 회전 조건:
left_tick_step = 3
right_tick_step = 7
예상 결과:
오른쪽 바퀴 이동량 > 왼쪽 바퀴 이동량
d_theta > 0
theta 증가
로봇은 반시계 방향으로 회전
제자리 회전 조건:
left_tick_step = -5
right_tick_step = 5
예상 결과:
d_center ≈ 0
d_theta 증가
로봇이 제자리에서 회전
13. 실제 로봇 보정 절차
1) Wheel Radius 보정
실제 로봇을 1 m 직진시킵니다.
그리고 실제 이동거리와 /odom의 x 이동량을 비교합니다.
예:
명령 거리 : 1.00 m
odom 거리 : 1.00 m
실제 거리 : 0.92 m
실제 로봇이 odom보다 덜 갔습니다.
이 경우 설정된 wheel_radius가 너무 큽니다.
보정식은 다음입니다.
new_wheel_radius = old_wheel_radius × 실제 이동 거리 / odom 이동 거리
예:
new_wheel_radius = 0.033 × 0.92 / 1.00
= 0.03036 m
반대로 odom은 0.92m라고 나오는데 실제로는 1.0m를 갔다면, wheel radius를 키워야 합니다.
new_wheel_radius = 0.033 × 1.00 / 0.92
= 0.03587 m
2) Wheel Separation 보정
로봇을 제자리에서 360도 회전시킵니다.
360도 = 2π rad
예:
odom은 360도 회전했다고 표시
실제 로봇은 330도만 회전
이 경우 실제 회전이 부족합니다.
wheel_separation 설정, 마찰, 미끄러짐, 캐스터 휠 간섭 등을 의심해야 합니다.
공식은 다음입니다.
d_theta = (d_right - d_left) / wheel_separation
wheel_separation이 작으면 회전각이 크게 계산됩니다.
wheel_separation이 크면 회전각이 작게 계산됩니다.
그래서 회전 보정은 실제 회전 테스트를 반복하면서 맞추는 것이 정석입니다.
14. Odometry Drift 확인 실습
Odometry drift는 시간이 지나면서 실제 위치와 추정 위치가 달라지는 현상입니다.
1) 실습 1: wheel_radius 일부러 틀리게 넣기
실제 바퀴 반지름이 0.033m인데 코드에는 0.040m로 넣습니다.
결과:
실제 이동거리 : 1.0 m
오도메트리 추정거리 : 약 1.2 m로봇이 실제보다 더 많이 이동했다고 추정합니다.
2) 실습 2: wheel_separation 일부러 틀리게 넣기
wheel_separation을 실제보다 작게 넣습니다.
공식은 다음입니다.
d_theta = (d_right - d_left) / wheel_separation분모가 작아지면 d_theta가 커집니다.
결과:
실제보다 많이 회전했다고 추정
RViz2에서 방향이 과하게 돌아감
3) 실습 3: 한쪽 바퀴 tick만 다르게 만들기
가상 엔코더에서 오른쪽 tick만 조금 크게 만듭니다.
left_tick_step = 5
right_tick_step = 6
로봇은 직진한다고 생각해도, odometry 계산상으로는 계속 회전합니다.
실제 로봇에서도 이런 일이 많이 생깁니다. 따라서 로봇의 직진성이 훼손되는 이유입니다.
원인은 다음과 같습니다.
좌우 바퀴 지름 차이
타이어 마모
모터 출력 차이
바닥 마찰 차이
엔코더 해상도 차이
기어 백래시
캐스터 휠 간섭
15. Covariance를 잘못 넣으면 생기는 문제
1) 너무 작게 넣는 경우
예:
x covariance = 0.000001
y covariance = 0.000001
yaw covariance = 0.000001
이렇게 하면 필터가 엔코더 오도메트리를 과신합니다.
문제:
IMU 보정값을 무시
LiDAR 보정값을 무시
SLAM 또는 localization이 불안정
위치 추정이 천천히 망가짐2) 너무 크게 넣는 경우
예:
x covariance = 100
y covariance = 100
yaw covariance = 100
이렇게 하면 필터가 엔코더 오도메트리를 거의 믿지 않습니다.
문제:
속도 추정이 흔들림
센서 융합 결과가 느림
Nav2 경로 추종이 부드럽지 않음
실전에서는 샘플 데이터를 보고 조정해야 합니다.
16. 자주 발생하는 문제와 해결 방향
1) 문제 1. /cmd_vel을 줬는데 로봇이 반대로 돈다
가능한 원인:
왼쪽/오른쪽 모터 배선 반대
엔코더 방향 반대
좌우 바퀴 이름 반대
angular.z 부호 처리 반대
정상 기준:
linear.x = 0.0
angular.z = 0.5
이 명령을 주면 ROS 표준 좌표계에서는 제자리 좌회전해야 합니다.
2) 문제 2. 직진하면 /odom이 많이 틀어진다
가능한 원인:
좌우 바퀴 반지름 차이
좌우 모터 속도 차이
엔코더 해상도 설정 오류
바퀴 미끄러짐
무게중심 불균형
해결 순서:
1. 좌우 tick 증가량 확인
2. 바퀴 지름 실측
3. ticks_per_revolution 확인
4. PID 속도 제어 적용
5. wheel_radius 보정
3) 문제 3. 직진은 맞는데 회전이 틀어진다
가능한 원인:
wheel_separation 설정 오류
바닥 마찰 차이
캐스터 휠 간섭
회전 중 미끄러짐
해결 순서:
1. 실제 좌우 바퀴 중심 간 거리 측정
2. 제자리 360도 회전 테스트
3. wheel_separation 보정
4. 저속 회전으로 테스트
4) 문제 4. encoder tick은 들어오는데 odometry가 이상하다
확인할 것:
tick 단위가 모터축 기준인지 바퀴축 기준인지
감속비가 반영되어 있는지
quadrature encoder의 x1, x2, x4 카운팅 방식
left/right 방향 부호
누적 tick overflow
시간 dt 계산
특히 감속기가 있는 DC 모터에서는 엔코더가 모터축에 붙어 있는 경우가 많습니다.
이때 바퀴축 1회전당 tick은 다음처럼 계산합니다.
wheel_ticks_per_revolution =
motor_encoder_ticks_per_revolution
× gear_ratio
× quadrature_multiplier예:
motor_encoder_ticks_per_revolution = 11
gear_ratio = 90
quadrature_multiplier = 4
wheel_ticks_per_revolution = 11 × 90 × 4
= 3960--> 4채배
5) 문제 5. /odom은 나오는데 TF가 안 나온다
확인할 것:
/tf 토픽이 발행되는가?
publish_tf 파라미터가 true인가?
sendTransform()이 실행되는가?
header.frame_id가 odom인가?
child_frame_id가 base_link인가?
같은 odom → base_link TF를 여러 노드가 동시에 발행하면 안 됩니다.
예를 들어 robot_localization이 TF를 발행한다면 encoder odometry 노드의 publish_tf는 false로 두는 것이 안전합니다.
17. 실전에서 권장하는 Odometry 구조
실제 모바일 로봇에서는 다음 구조가 안정적입니다.
Wheel Encoder
↓
/wheel/odom
↓
robot_localization + IMU
↓
/odometry/filtered
↓
Nav2 / SLAM / RViz2
TF 구조는 다음입니다.
odom
└── base_link
주의할 점은 다음입니다.
/odom을 발행하는 노드가 TF도 발행할지 결정해야 합니다.
robot_localization이 odom → base_link를 발행한다면 encoder odom node는 TF를 꺼야 합니다.
같은 TF를 여러 노드가 동시에 발행하면 안 됩니다.
18. 요약
Wheel Radius
바퀴 반지름
tick을 거리로 바꿀 때 핵심
직진 거리 정확도에 큰 영향Wheel Separation
좌우 구동 바퀴 중심 사이 거리
좌우 바퀴 이동거리 차이를 회전각으로 바꿀 때 핵심
회전 정확도에 큰 영향/cmd_vel
로봇 중심 속도 명령
linear.x는 전진 속도 [m/s]
angular.z는 회전 속도 [rad/s]Wheel Velocity
v_l = v - ωL / 2
v_r = v + ωL / 2Encoder Tick
바퀴 또는 모터 회전량
거리 변환 필요
distance = delta_ticks × 2πr / ticks_per_revolutionOdometry
d_center = (d_right + d_left) / 2
d_theta = (d_right - d_left) / wheel_separationTF
odom → base_linkDrift
엔코더 기반 위치 추정 오차가 누적되는 현상
완전히 없앨 수는 없고 보정과 센서 융합으로 줄인다Covariance
센서 추정값의 불확실성
작으면 신뢰
크면 불신'강좌 > ROS2' 카테고리의 다른 글
| 10일차 강의 (0) | 2026.05.31 |
|---|---|
| launch 파일에서 실행 인자 사용하기 #2 (0) | 2026.05.29 |
| GIMP를 이용한 지도 파일 .pgm 수정 (0) | 2026.05.29 |
| Cancel 테스트용 Action Client 만들기 (0) | 2026.05.26 |
| ROS 2 Python Topic 실습 : RobotStatus 메시지로 로봇 상태 주고받기 (0) | 2026.05.26 |
