TurtleBot3 Odometry 분석

1. TurtleBot3 시뮬레이터에서 Odometry 확인하기

TurtleBot3는 Gazebo 기반 시뮬레이션을 제공합니다. TurtleBot3 Simulation Package는 turtlebot3와 turtlebot3_msgs 패키지를 필요로 하며, Gazebo 환경에서는 IMU, LDS, 카메라 같은 센서를 사용할 수 있어 SLAM이나 Navigation 실습에 적합합니다.

 

기본 실행 흐름은 다음과 같습니다.

 

cd ~/turtlebot3_ws/src/
git clone -b humble https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/turtlebot3_ws
colcon build --symlink-install

 

 

환경 설정:

 

source /opt/ros/humble/setup.bash
source ~/turtlebot3_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger

 

 

Gazebo 월드 실행:

 

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

 

 

키보드 조종 노드 실행:

 

ros2 run turtlebot3_teleop teleop_keyboard

 

 

Odometry 토픽 확인:

 

ros2 topic list | grep odom

 

 

ros2 topic echo /odom

 

 

출력 예시는 대략 이런 구조입니다.

 

header:
  frame_id: odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.34
      y: 0.02
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.12
      w: 0.99
twist:
  twist:
    linear:
      x: 0.20
    angular:
      z: 0.0

 

 

여기서 pose.pose.position.x와 pose.pose.position.y는 로봇의 위치, orientation은 회전 방향, twist.twist.linear.x는 전진 속도, twist.twist.angular.z는 회전 속도입니다.

 

RViz2로 확인하려면 다음 명령을 실행합니다.

 

ros2 launch turtlebot3_bringup rviz2.launch.py

 

Gazebo 실행 중 RViz2를 통해 시뮬레이션 데이터를 시각화할 수 있습니다.

 

 

 

2. TurtleBot3 Odometry 소스 구조

TurtleBot3의 Odometry 핵심 소스는 다음 파일입니다.

turtlebot3_node/src/odometry.cpp
turtlebot3_node/include/turtlebot3_node/odometry.hpp

 

 

Odometry 클래스는 생성자에서 다음 정보를 초기화합니다.

 

Odometry::Odometry(
  std::shared_ptr<rclcpp::Node> & nh,
  const double wheels_separation,
  const double wheels_radius)

 

 

여기서 중요한 인자는 두 개입니다.

 

wheels_separation	좌우 바퀴 사이 거리
wheels_radius	바퀴 반지름

 

이 두 값은 차동 구동 로봇의 위치 계산에서 핵심입니다. 바퀴 반지름을 알아야 바퀴 회전량을 실제 이동 거리로 바꿀 수 있고, 좌우 바퀴 간격을 알아야 회전 각도를 계산할 수 있습니다.

 

생성자에서는 robot_pose_와 robot_vel_도 초기화합니다.

 

robot_pose_({0.0, 0.0, 0.0}),
robot_vel_({0.0, 0.0, 0.0})

 

 

즉, 로봇의 시작 위치는 (x=0, y=0, theta=0)이고, 시작 속도도 0입니다.

 

 

 

3. 주요 파라미터 분석

TurtleBot3 Odometry 소스에서는 다음 파라미터를 사용합니다.

 

odometry.frame_id
odometry.child_frame_id
namespace
odometry.use_imu
odometry.publish_tf

 

 

소스에서는 기본값으로 frame_id_of_odometry_를 "odom", child_frame_id_of_odometry_를 "base_footprint"로 설정합니다.

odometry.frame_id	기준 좌표계, 보통 odom
odometry.child_frame_id	로봇 본체 좌표계, 보통 base_footprint
odometry.use_imu	IMU 각도를 사용할지 여부
odometry.publish_tf	odom → base_footprint TF를 발행할지 여부
namespace	멀티로봇 환경에서 로봇별 네임스페이스 구분

 

odom은 로봇이 처음 출발한 위치를 기준으로 하는 지역 좌표계입니다. base_footprint는 로봇 몸체의 바닥 중심 좌표계입니다. 따라서 Odometry는 결국 다음 관계를 계산하는 작업입니다.

 

odom 좌표계 기준으로 base_footprint가 어디에 있는가?

 

 

 

 

4. 입력 데이터: joint_states와 imu

TurtleBot3 Odometry는 기본적으로 /joint_states를 사용합니다. use_imu가 켜져 있으면 /joint_states와 /imu를 함께 동기화해서 사용합니다.

소스에서는 use_imu_가 참이면 message_filters를 이용해 joint_states와 imu를 동기화합니다. 반대로 use_imu_가 거짓이면 단순히 joint_states만 구독합니다.

 

if (use_imu_) {
  // joint_states + imu 동기화
} else {
  joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
    "joint_states",
    qos,
    std::bind(&Odometry::joint_state_callback, this, std::placeholders::_1));
}

 

 

즉, TurtleBot3 Odometry 계산 방식은 두 가지입니다.

IMU 미사용	좌우 바퀴 회전량 차이로 계산
IMU 사용	IMU orientation에서 yaw 각도 계산

 

처음 odometry를 테스트 할 때에는 IMU 없이 바퀴 기반 Odometry를 사용하는 것이 좋습니다. 구조가 단순하고 원리가 명확합니다.

 

 

    joint_state_imu_sync_->connectInput(*msg_ftr_joint_state_sub_, *msg_ftr_imu_sub_);

    joint_state_imu_sync_->setInterMessageLowerBound(
      0,
      rclcpp::Duration(75ms));

    joint_state_imu_sync_->setInterMessageLowerBound(
      1,
      rclcpp::Duration(15ms));

    joint_state_imu_sync_->registerCallback(
      std::bind(
        &Odometry::joint_state_and_imu_callback,
        this,
        std::placeholders::_1,
        std::placeholders::_2));

 

 

ROS 2 message_filters의 Synchronizer를 이용해서 JointState 메시지와 IMU 메시지를 시간 기준으로 묶어서 하나의 콜백으로 처리하는 부분입니다.

 

 

joint_state_imu_sync_->connectInput(
  *msg_ftr_joint_state_sub_,
  *msg_ftr_imu_sub_);

 

 

여기서는 동기화 객체 joint_state_imu_sync_에 두 개의 입력 토픽을 연결합니다.

 

즉,

 

msg_ftr_joint_state_sub_
msg_ftr_imu_sub_

 

 

두 subscriber에서 들어오는 메시지를 joint_state_imu_sync_가 받아서 서로 시간적으로 맞는 메시지 쌍을 찾게 됩니다.

 

보통 타입은 이런 구조일 가능성이 큽니다.

 

message_filters::Subscriber<sensor_msgs::msg::JointState>
message_filters::Subscriber<sensor_msgs::msg::Imu>

 

 

joint_state_imu_sync_->setInterMessageLowerBound(
  0,
  rclcpp::Duration(75ms));

 

 

첫 번째 입력 메시지에 대해 최소 메시지 간격을 설정합니다.

 

여기서 0은 connectInput()에 넣은 첫 번째 subscriber를 의미합니다.

 

*msg_ftr_joint_state_sub_

 

 

즉, JointState 메시지는 최소 75ms 간격으로 들어오는 것으로 취급하거나, 그보다 더 촘촘하게 들어오는 메시지는 동기화 처리에서 제한할 수 있습니다.

 

75ms는 주파수로 보면 대략 다음과 같습니다.

1 / 0.075초 = 약 13.3Hz

 

 

즉, JointState는 약 13Hz 이하의 입력 간격을 기대하는 설정입니다.

 

 

joint_state_imu_sync_->setInterMessageLowerBound(
  1,
  rclcpp::Duration(15ms));

 

 

두 번째 입력 메시지에 대해 최소 메시지 간격을 설정합니다.

여기서 1은 connectInput()에 넣은 두 번째 subscriber를 의미합니다.

 

*msg_ftr_imu_sub_

 

 

즉, IMU 메시지는 최소 15ms 간격으로 들어오는 것으로 설정합니다.

15ms는 주파수로 보면 대략 다음과 같습니다.

1 / 0.015초 = 약 66.7Hz

 

 

즉, IMU는 약 66Hz 이하의 메시지 간격을 기대하는 설정입니다.

 

 

joint_state_imu_sync_->registerCallback(
  std::bind(
    &Odometry::joint_state_and_imu_callback,
    this,
    std::placeholders::_1,
    std::placeholders::_2));

 

 

동기화된 메시지 쌍이 만들어졌을 때 실행할 콜백 함수를 등록합니다.

 

등록되는 함수는 이것입니다.

 

Odometry::joint_state_and_imu_callback

 

 

이 함수는 클래스 Odometry의 멤버 함수입니다.

std::bind()는 멤버 함수를 콜백으로 넘기기 위해 사용합니다.

 

this

 

 

는 현재 Odometry 객체를 의미합니다.

 

std::placeholders::_1
std::placeholders::_2

 

 

는 동기화되어 들어오는 두 개의 메시지를 의미합니다.

 

즉, 실제 콜백 함수는 대략 이런 형태일 가능성이 높습니다.

 

void Odometry::joint_state_and_imu_callback(
  const sensor_msgs::msg::JointState::ConstSharedPtr joint_state_msg,
  const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg)
{
  // JointState + IMU 기반 odometry 계산
}

 

 

 

 

 

5. 콜백 함수 흐름

Odometry의 기본 콜백 흐름은 다음과 같습니다.

 

void Odometry::joint_state_callback(
  const sensor_msgs::msg::JointState::SharedPtr joint_state_msg)
{
  const rclcpp::Time current_time = joint_state_msg->header.stamp;
  static rclcpp::Time last_time = current_time;
  const rclcpp::Duration duration = current_time - last_time;

  update_joint_state(joint_state_msg);
  calculate_odometry(duration);
  publish(current_time);

  last_time = current_time;
}

 

 

 

주요 흐름은 아래와 같습니다.

1. 현재 시간 읽기
2. 이전 시간과 비교해서 dt 계산
3. joint_state에서 바퀴 회전 변화량 계산
4. Odometry 계산
5. /odom 메시지 발행
6. 현재 시간을 last_time으로 저장

 

 

이 흐름은 실제 로봇 제어 프로그램에서도 자주 사용합니다.

센서 입력 → 변화량 계산 → 상태 추정 → 토픽 발행

 

 

로봇 개발에서는 이 구조를 제대로 이해해야 합니다. 나중에 엔코더, IMU, GPS, LiDAR, UWB 같은 센서를 붙여도 결국 이 패턴을 확장하는 방식으로 갑니다.

 

 

 

6. Odometry의 publish()

 

calculate_odometry() 함수가 로봇의 위치와 속도를 계산한다면, publish() 함수는 그 결과를 ROS 2 표준 메시지와 TF로 외부에 내보내는 역할을 합니다.

 

즉, 전체 흐름에서 publish()는 마지막 단계입니다.

바퀴 회전량 계산
        ↓
x, y, theta 계산
        ↓
linear velocity, angular velocity 계산
        ↓
publish() 호출
        ↓
/odom 토픽 발행
        ↓
필요하면 odom → base_footprint TF 발행

 

 

소스는 다음 구조로 되어 있습니다.

 

void Odometry::publish(const rclcpp::Time & now)
{
  auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();

  ...
}

 

여기서 nav_msgs::msg::Odometry 메시지를 생성합니다.

 

Odometry 메시지는 ROS에서 로봇의 위치와 속도를 표현할 때 사용하는 표준 메시지입니다. 크게 보면 다음 정보를 담습니다.

header
pose
twist

 

header	시간과 기준 좌표계 정보
pose	로봇의 위치와 자세
twist	로봇의 선속도와 각속도

 

 

 

1) Odometry 메시지의 frame 설정

 

먼저 메시지의 좌표계 정보를 설정합니다.

 

odom_msg->header.frame_id = frame_id_of_odometry_;
odom_msg->child_frame_id = child_frame_id_of_odometry_;
odom_msg->header.stamp = now;

 

 

여기서 frame_id_of_odometry_는 보통 "odom"입니다.

child_frame_id_of_odometry_는 보통 "base_footprint"입니다.

 

즉, 이 메시지는 다음 의미를 가집니다.

odom 좌표계 기준으로 base_footprint가 어디에 있는가?

 

 

조금 더 쉽게 말하면:

로봇이 처음 출발한 odom 좌표계 기준에서
현재 로봇 본체 base_footprint의 위치와 방향은 얼마인가?

 

 

header.stamp = now는 이 Odometry 정보가 어느 시점의 데이터인지 표시합니다. ROS 2에서 시간 정보는 매우 중요합니다. TF, 센서 데이터, SLAM, Navigation2는 모두 시간 동기화를 기반으로 동작하기 때문입니다.

 

 

 

2) 로봇 위치 입력

 

다음은 위치 정보를 넣는 부분입니다.

 

odom_msg->pose.pose.position.x = robot_pose_[0];
odom_msg->pose.pose.position.y = robot_pose_[1];
odom_msg->pose.pose.position.z = 0;

 

 

robot_pose_ 배열은 앞의 calculate_odometry() 함수에서 계산된 값입니다.

robot_pose_[0] = x 위치
robot_pose_[1] = y 위치
robot_pose_[2] = yaw 각도

 

 

TurtleBot3는 바닥 위를 이동하는 2D 모바일 로봇이므로 z 위치는 0으로 고정합니다.

 

odom_msg->pose.pose.position.z = 0;

 

 

즉 TurtleBot3의 Odometry는 기본적으로 3차원 위치 추정이 아니라 2차원 평면 이동 추정입니다.

 

 

 

3) yaw 각도를 quaternion으로 변환

 

다음 코드는 회전 정보를 설정하는 부분입니다.

 

tf2::Quaternion q;
q.setRPY(0.0, 0.0, robot_pose_[2]);

 

 

TurtleBot3는 평면 위에서 움직이므로 roll, pitch는 사용하지 않습니다.

roll  = 0.0
pitch = 0.0
yaw   = robot_pose_[2]

 

 

따라서 setRPY()에는 다음 값이 들어갑니다.

 

q.setRPY(0.0, 0.0, robot_pose_[2]);

 

 

여기서 robot_pose_[2]는 로봇의 회전 방향, 즉 yaw입니다.

 

ROS에서는 자세를 표현할 때 Euler angle보다 quaternion을 많이 사용합니다. 그래서 yaw 값을 바로 넣지 않고 quaternion으로 변환한 뒤 메시지에 저장합니다.

 

odom_msg->pose.pose.orientation.x = q.x();
odom_msg->pose.pose.orientation.y = q.y();
odom_msg->pose.pose.orientation.z = q.z();
odom_msg->pose.pose.orientation.w = q.w();

 

 

정리하면 이 부분의 의미는 다음과 같습니다.

로봇의 현재 방향 yaw를 quaternion 형식으로 변환해서
Odometry 메시지의 orientation에 저장한다.

 

 

2D 로봇이라도 ROS 메시지에서는 orientation을 quaternion으로 넣어야 합니다. RViz, TF, Navigation2가 이 값을 사용해 로봇의 방향을 판단합니다.

 

 

 

4) 로봇 속도 입력

다음은 속도 정보를 넣는 부분입니다.

 

odom_msg->twist.twist.linear.x = robot_vel_[0];
odom_msg->twist.twist.angular.z = robot_vel_[2];

 

 

robot_vel_도 calculate_odometry()에서 계산된 값입니다.

robot_vel_[0] = 전진 속도, linear x
robot_vel_[1] = 측면 속도, linear y
robot_vel_[2] = 회전 속도, angular z

 

 

TurtleBot3는 차동 구동 로봇이므로 옆으로 움직이지 못합니다. 그래서 실제로 중요한 속도는 두 개입니다.

linear.x   = 전진/후진 속도
angular.z  = 좌회전/우회전 속도

 

 

코드에서도 이 두 값만 메시지에 넣습니다.

 

odom_msg->twist.twist.linear.x = robot_vel_[0];
odom_msg->twist.twist.angular.z = robot_vel_[2];

 

 

linear.y, linear.z, angular.x, angular.y는 사용하지 않습니다.

 

 

 

5) Covariance 주석 부분

중간에 다음 주석이 있습니다.

 

// TODO(Will Son): Find more accurate covariance.

 

 

그리고 아래에 covariance 값들이 주석 처리되어 있습니다.

 

// odom_msg->pose.covariance[0] = 0.05;
// odom_msg->pose.covariance[7] = 0.05;
// odom_msg->pose.covariance[14] = 1.0e-9;
// odom_msg->pose.covariance[21] = 1.0e-9;
// odom_msg->pose.covariance[28] = 1.0e-9;
// odom_msg->pose.covariance[35] = 0.0872665;

 

 

Covariance는 쉽게 말하면 이 값이 얼마나 믿을 만한가를 나타내는 정보입니다.

 

Odometry 메시지에는 pose covariance와 twist covariance가 있습니다.

pose.covariance  = 위치와 자세 추정값의 불확실성
twist.covariance = 속도 추정값의 불확실성

 

 

배열은 6x6 행렬을 1차원 배열로 펼친 구조입니다.

 

순서는 일반적으로 다음과 같습니다.

x, y, z, roll, pitch, yaw

 

 

따라서 주요 인덱스는 다음 의미를 가집니다.

0	x 위치 불확실성
7	y 위치 불확실성
14	z 위치 불확실성
21	roll 불확실성
28	pitch 불확실성
35	yaw 불확실성

 

주석 처리된 값 중 예를 들면:

 

// odom_msg->pose.covariance[0] = 0.05;

 

 

이것은 x 위치 추정에 대한 불확실성을 의미합니다.

 

// odom_msg->pose.covariance[35] = 0.0872665;

 

 

이것은 yaw 회전 추정에 대한 불확실성을 의미합니다.

 

현재 TurtleBot3 소스에서는 이 값들이 주석 처리되어 있습니다. 이유는 주석 그대로입니다.

더 정확한 covariance 값을 찾아야 한다.

 

 

실제 로봇에서는 바닥 상태, 바퀴 미끄러짐, 엔코더 해상도, IMU 품질 등에 따라 Odometry 신뢰도가 달라집니다. 그래서 covariance를 아무 값이나 넣으면 오히려 SLAM이나 Localization 성능에 안 좋은 영향을 줄 수 있습니다.

특히 robot_localization, EKF, UKF 같은 센서 퓨전 패키지를 사용할 때 covariance는 중요합니다.

 

간단히 말하면:

covariance가 작다  → 이 센서 값을 많이 믿는다
covariance가 크다  → 이 센서 값을 덜 믿는다

 

 

그래서 실전에서는 covariance를 튜닝해야 합니다.

 

 

 

6) TF 메시지 생성

다음 부분은 TF를 만들기 위한 코드입니다.

 

geometry_msgs::msg::TransformStamped odom_tf;

 

 

TransformStamped는 두 좌표계 사이의 위치와 회전 관계를 표현하는 메시지입니다.

여기서는 /odom 메시지에 넣은 위치와 방향을 그대로 TF에도 넣습니다.

 

odom_tf.transform.translation.x = odom_msg->pose.pose.position.x;
odom_tf.transform.translation.y = odom_msg->pose.pose.position.y;
odom_tf.transform.translation.z = odom_msg->pose.pose.position.z;
odom_tf.transform.rotation = odom_msg->pose.pose.orientation;

 

 

즉, TF의 translation은 로봇 위치입니다.

x = 현재 로봇 x 위치
y = 현재 로봇 y 위치
z = 0

 

 

TF의 rotation은 로봇 방향입니다.

rotation = 현재 로봇 yaw를 quaternion으로 변환한 값

 

 

 

 

7) TF의 frame 설정

 

다음은 TF의 부모 좌표계와 자식 좌표계를 설정하는 부분입니다.

 

odom_tf.header.frame_id = frame_id_of_odometry_;
odom_tf.child_frame_id = child_frame_id_of_odometry_;
odom_tf.header.stamp = now;

 

 

이것도 /odom 메시지와 같은 구조입니다.

parent frame = odom
child frame  = base_footprint

 

 

즉, 이 TF는 다음 변환을 의미합니다.

odom → base_footprint

 

 

이 관계가 있어야 RViz에서 로봇 위치가 올바르게 보이고, Navigation2나 SLAM도 좌표계를 정상적으로 연결할 수 있습니다.

ROS 로봇 시스템에서 /odom 토픽과 TF는 비슷한 정보를 담지만 용도가 다릅니다.

/odom 토픽	로봇의 위치, 속도 데이터를 메시지로 제공
TF	좌표계 사이의 관계를 실시간으로 제공

 

/odom은 데이터 메시지이고, TF는 좌표 변환 시스템입니다.

 

 

 

8) /odom 토픽 발행

 

다음 코드가 실제로 Odometry 메시지를 발행하는 부분입니다.

 

odom_pub_->publish(std::move(odom_msg));

 

 

여기서 std::move(odom_msg)를 사용합니다.

 

odom_msg는 std::make_unique로 생성된 unique_ptr입니다.

 

auto odom_msg = std::make_unique<nav_msgs::msg::Odometry>();

 

 

unique_ptr는 복사할 수 없고 소유권을 이동해야 합니다. 그래서 std::move()를 사용해 퍼블리셔에게 메시지 소유권을 넘깁니다.

 

쉽게 말하면:

메시지를 복사하지 않고 퍼블리셔로 넘긴다.

 

 

ROS 2 C++ 코드에서는 성능을 위해 이런 방식이 자주 사용됩니다.

 

 

 

9) TF 발행 여부 확인

 

마지막 부분입니다.

 

 

if (publish_tf_) {
  tf_broadcaster_->sendTransform(odom_tf);
}

 

 

publish_tf_가 true일 때만 TF를 발행합니다.

이 옵션이 필요한 이유는 시스템 구성에 따라 TF를 다른 노드에서 발행할 수도 있기 때문입니다.

 

예를 들어 어떤 로봇 시스템에서는 다음과 같이 나눌 수 있습니다.

Odometry 노드       → /odom 토픽만 발행
robot_localization → odom → base_link TF 발행

 

 

또는 TurtleBot3 기본 구조처럼 Odometry 노드가 직접 TF를 발행할 수도 있습니다.

Odometry 노드 → /odom 발행 + odom → base_footprint TF 발행

 

 

TF를 중복 발행하면 문제가 생깁니다. 같은 좌표계 변환을 여러 노드가 동시에 발행하면 RViz나 Navigation2에서 좌표계가 흔들리거나 경고가 발생할 수 있습니다.

그래서 publish_tf_ 옵션이 있습니다.

 

 

 

 

 

7. 바퀴 회전량 계산: update_joint_state()

update_joint_state() 함수는 이전 바퀴 위치와 현재 바퀴 위치의 차이를 계산합니다.

 

diff_joint_positions_[0] = joint_state->position[0] - last_joint_positions[0];
diff_joint_positions_[1] = joint_state->position[1] - last_joint_positions[1];

last_joint_positions[0] = joint_state->position[0];
last_joint_positions[1] = joint_state->position[1];

 

 

소스에서는 last_joint_positions를 static 변수로 두고, 매번 새 joint position과 비교합니다.

 

여기서 joint_state->position[0], joint_state->position[1]은 각각 왼쪽, 오른쪽 바퀴의 누적 회전 위치라고 보면 됩니다. 단위는 라디안입니다.

 

예를 들어 다음과 같다고 가정합니다.

왼쪽 바퀴	1.0 rad	1.2 rad	0.2 rad
오른쪽 바퀴	1.0 rad	1.2 rad	0.2 rad

 

두 바퀴가 같은 만큼 돌았으므로 로봇은 직진합니다.

 

반대로 다음과 같다면 회전이 발생합니다.

 

왼쪽 바퀴	1.0 rad	1.1 rad	0.1 rad
오른쪽 바퀴	1.0 rad	1.3 rad	0.3 rad

 

오른쪽 바퀴가 더 많이 돌았으므로 로봇은 왼쪽 방향으로 회전합니다.

 

 

 

 

8. IMU yaw 계산: update_imu()

IMU를 사용할 경우 update_imu()에서 quaternion orientation을 yaw 각도로 변환합니다.

 

imu_angle_ = atan2f(
  imu->orientation.x * imu->orientation.y + imu->orientation.w * imu->orientation.z,
  0.5f - imu->orientation.y * imu->orientation.y - imu->orientation.z * imu->orientation.z);

 

 

이 값은 로봇의 z축 회전, 즉 yaw 각도입니다.

 

바퀴 기반 Odometry는 바닥 미끄러짐에 약합니다. 예를 들어 로봇이 회전 중에 바퀴가 살짝 헛돌면 실제 회전량과 엔코더 기반 회전량이 달라집니다. 이때 IMU yaw를 사용하면 회전 방향 추정이 조금 더 안정될 수 있습니다.

 

다만 IMU도 완벽하지 않습니다. 저가형 IMU는 노이즈와 드리프트가 있습니다. 그래서 실제 고급 로봇에서는 바퀴 엔코더, IMU, LiDAR, 카메라, GPS 등을 함께 사용해서 센서 퓨전을 합니다.

 

 

 

 

9. Odometry 계산: calculate_odometry()

Odometry의 진짜 핵심은 calculate_odometry() 함수입니다.

먼저 왼쪽, 오른쪽 바퀴의 변화량을 가져옵니다.

 

double wheel_l = diff_joint_positions_[0];
double wheel_r = diff_joint_positions_[1];

 

 

그리고 시간 간격을 계산합니다.

 

double step_time = duration.seconds();

if (step_time == 0.0) {
  return false;
}

 

 

시간 간격이 0이면 속도 계산이 불가능하므로 함수를 종료합니다.

 

 

 

 

1) 이동 거리 계산

TurtleBot3 소스에서는 이동 거리 delta_s를 다음과 같이 계산합니다.

 

delta_s = wheels_radius_ * (wheel_r + wheel_l) / 2.0;

 

 

수식으로 쓰면 다음과 같습니다.

delta_s = R * (Δright + Δleft) / 2

 

R	바퀴 반지름
Δright	오른쪽 바퀴 회전 변화량
Δleft	왼쪽 바퀴 회전 변화량

 

바퀴 회전량은 라디안이고, 바퀴 반지름을 곱하면 실제 이동 거리 m 단위가 됩니다.

 

예를 들어 바퀴 반지름이 0.033m, 양쪽 바퀴가 각각 1 rad 회전했다면:

delta_s = 0.033 * (1 + 1) / 2
delta_s = 0.033m

 

 

즉 약 3.3cm 전진한 것입니다.

 

 

 

2). 회전 각도 계산

IMU를 사용하지 않는 경우 회전량은 다음과 같이 계산합니다.

 

theta = wheels_radius_ * (wheel_r - wheel_l) / wheels_separation_;
delta_theta = theta;

 

 

수식으로 쓰면 다음과 같습니다.

delta_theta = R * (Δright - Δleft) / L

 

R	바퀴 반지름
L	좌우 바퀴 사이 거리
Δright	오른쪽 바퀴 회전 변화량
Δleft	왼쪽 바퀴 회전 변화량

 

오른쪽 바퀴와 왼쪽 바퀴가 같은 만큼 돌면 Δright - Δleft = 0이므로 회전량은 0입니다. 즉 직진입니다.

 

오른쪽 바퀴가 더 많이 돌면 양수 회전이 발생하고, 왼쪽 바퀴가 더 많이 돌면 음수 회전이 발생합니다.

 

IMU를 사용하는 경우에는 바퀴 차이 대신 IMU yaw 변화량을 사용합니다.

 

theta = imu_angle_;
delta_theta = theta - last_theta;

 

 

TurtleBot3 소스는 use_imu_ 값에 따라 IMU 기반 회전 계산과 바퀴 기반 회전 계산을 나누어 처리합니다.

 

 

 

3) 위치 누적 계산

이제 이동 거리와 회전량을 알았으므로 로봇의 위치를 갱신합니다.

 

robot_pose_[0] += delta_s * cos(robot_pose_[2] + (delta_theta / 2.0));
robot_pose_[1] += delta_s * sin(robot_pose_[2] + (delta_theta / 2.0));
robot_pose_[2] += delta_theta;

 

 

이 부분이 Odometry의 핵심입니다.

 

의미는 다음과 같습니다.

x     = x + 이동거리 * cos(현재방향 + 회전량/2)
y     = y + 이동거리 * sin(현재방향 + 회전량/2)
theta = theta + 회전량

 

 

여기서 delta_theta / 2.0을 더하는 이유는 로봇이 이동하는 동안 방향이 서서히 변하기 때문입니다. 단순히 이동 전 방향만 쓰는 것보다, 이동 중간 방향을 사용하면 곡선 이동을 조금 더 자연스럽게 근사할 수 있습니다.

 

이 방식은 차동 구동 로봇에서 자주 쓰이는 기본적인 Odometry 적분 방식입니다.

 

 

 

4) 속도 계산

위치 계산 후에는 속도를 계산합니다.

 

v = delta_s / step_time;
w = delta_theta / step_time;

robot_vel_[0] = v;
robot_vel_[1] = 0.0;
robot_vel_[2] = w;

 

 

v	선속도, m/s
w	각속도, rad/s
robot_vel_[0]	x 방향 속도
robot_vel_[1]	y 방향 속도
robot_vel_[2]	z축 회전 속도

 

TurtleBot3는 일반적인 차동 구동 로봇이므로 측면 이동이 불가능합니다. 그래서 robot_vel_[1] = 0.0입니다. 즉, 메카넘 휠이나 옴니휠처럼 옆으로 이동하는 로봇이 아닙니다.

 

 

 

 

10. /odom 메시지 발행

계산된 위치와 속도는 publish() 함수에서 /odom 메시지로 발행됩니다.

 

odom_msg->header.frame_id = frame_id_of_odometry_;
odom_msg->child_frame_id = child_frame_id_of_odometry_;
odom_msg->pose.pose.position.x = robot_pose_[0];
odom_msg->pose.pose.position.y = robot_pose_[1];
odom_msg->twist.twist.linear.x = robot_vel_[0];
odom_msg->twist.twist.angular.z = robot_vel_[2];

 

 

또한 robot_pose_[2], 즉 yaw 값을 quaternion으로 변환합니다.

 

tf2::Quaternion q;
q.setRPY(0.0, 0.0, robot_pose_[2]);

 

 

ROS에서는 회전을 roll, pitch, yaw 그대로 메시지에 넣지 않고 quaternion으로 넣는 경우가 많습니다. 그래서 yaw 값을 quaternion으로 바꾼 뒤 orientation에 저장합니다. TurtleBot3 소스도 이 방식을 사용합니다.

 

 

 

 

11. TF 발행

Odometry는 /odom 토픽만 중요한 것이 아닙니다. TF도 중요합니다.

 

소스에서는 publish_tf_가 참이면 odom → base_footprint 변환을 발행합니다.

 

if (publish_tf_) {
  tf_broadcaster_->sendTransform(odom_tf);
}

 

 

이 TF가 있어야 RViz, SLAM, Navigation2가 로봇의 좌표 관계를 이해할 수 있습니다. TurtleBot3 소스에서는 odom_tf의 translation과 rotation을 /odom 메시지의 pose 값과 동일하게 구성한 뒤, 옵션에 따라 TF를 발행합니다.

 

좌표 관계를 단순화하면 다음과 같습니다.

map
 └── odom
      └── base_footprint
           └── base_link
                └── laser, camera, imu ...

 

 

Odometry는 이 중에서 주로 다음 관계를 담당합니다.

odom → base_footprint

 

 

SLAM이나 Navigation을 하면 map → odom 관계가 추가됩니다. 이때 Odometry는 짧은 시간 동안의 부드러운 이동 추정을 담당하고, SLAM은 장기적인 위치 보정을 담당한다고 보면 됩니다.

 

 

 

 

12. Odometry Reset 서비스

TurtleBot3 Odometry 소스에는 reset_odometry 서비스도 있습니다.

 

reset_odom_srv_ = nh_->create_service<std_srvs::srv::Trigger>(
  "reset_odometry",
  ...
);

 

 

서비스가 호출되면 다음 코드가 실행됩니다.

 

robot_pose_ = {0.0, 0.0, 0.0};
robot_vel_ = {0.0, 0.0, 0.0};
response->success = true;
response->message = "Odometry reset";

 

 

즉, 현재까지 누적된 위치와 속도를 0으로 초기화합니다.

실습에서는 다음 명령으로 호출할 수 있습니다.

 

ros2 service call /reset_odometry std_srvs/srv/Trigger

 

 

로봇 실험을 반복할 때 유용합니다. 예를 들어 같은 출발점에서 여러 번 주행 테스트를 할 때 매번 Odometry를 초기화하고 시작할 수 있습니다.

 

 

 

 

13. 전체 동작 흐름 정리

 

TurtleBot3 Odometry의 전체 흐름은 다음과 같습니다.

/joint_states 수신
        ↓
왼쪽/오른쪽 바퀴 회전 변화량 계산
        ↓
바퀴 반지름으로 실제 이동 거리 계산
        ↓
좌우 바퀴 차이 또는 IMU로 회전량 계산
        ↓
x, y, theta 누적
        ↓
선속도, 각속도 계산
        ↓
/odom 발행
        ↓
필요하면 odom → base_footprint TF 발행

 

 

핵심 수식은 세 개입니다.

delta_s = R * (Δright + Δleft) / 2

delta_theta = R * (Δright - Δleft) / L

x += delta_s * cos(theta + delta_theta / 2)
y += delta_s * sin(theta + delta_theta / 2)
theta += delta_theta

 

 

이 세 수식만 제대로 이해하면 TurtleBot3 Odometry 소스의 큰 줄기는 잡힙니다.

 

 

 

 

14. /odom을 구독해서 이동 거리 출력하기

Odometry를 단순히 보는 것에서 끝내지 말고, 직접 /odom을 구독하는 노드를 만들어보면 이해가 훨씬 빨라집니다.

 

[ Python 예제 ]  - odom_monitor.py

 

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
import math


class OdomMonitor(Node):
    def __init__(self):
        super().__init__('odom_monitor')
        self.sub = self.create_subscription(
            Odometry,
            '/odom',
            self.odom_callback,
            10
        )

    def odom_callback(self, msg):
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

        q = msg.pose.pose.orientation
        yaw = math.atan2(
            2.0 * (q.w * q.z + q.x * q.y),
            1.0 - 2.0 * (q.y * q.y + q.z * q.z)
        )

        linear_x = msg.twist.twist.linear.x
        angular_z = msg.twist.twist.angular.z

        self.get_logger().info(
            f'x={x:.3f}, y={y:.3f}, yaw={yaw:.3f}, '
            f'v={linear_x:.3f}, w={angular_z:.3f}'
        )


def main():
    rclpy.init()
    node = OdomMonitor()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

 

 

my_first_package에 odom_monitor.py 파일을 생성합니다.

 

touch odom_monitor.py

 

 

Python 코드를 작성합니다.

 

 

패키지의 setup.py를 수정합니다.

 

entry_points 부분을 다음처럼 수정합니다.

 

entry_points={
    'console_scripts': [
        'odom_monitor = odom_monitor.odom_monitor:main',
    ],
},

 

 

빌드합니다.

 

cd ~/ros2_study
colcon build --packages-select my_first_package

 

 

환경 설정을 다시 적용합니다.

 

source install/setup.bash

 

 

이제 TurtleBot3 시뮬레이터를 실행합니다.

 

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

 

 

새 터미널을 열고 키보드 조종 노드를 실행합니다.

 

export TURTLEBOT3_MODEL=burger
ros2 run turtlebot3_teleop teleop_keyboard

 

 

또 다른 새 터미널에서 방금 만든 Odometry 모니터 노드를 실행합니다.

 

ros2 run my_first_package odom_monitor

 

 

TurtleBot3를 키보드로 움직이면 다음과 비슷한 로그가 출력됩니다.

 

 

x=0.125, y=0.003, yaw=0.021, v=0.200, w=0.000
x=0.143, y=0.004, yaw=0.021, v=0.200, w=0.000
x=0.160, y=0.005, yaw=0.022, v=0.200, w=0.000

 

 

 

 

 

15. Odometry의 한계

Odometry는 필수지만 완벽하지 않습니다. 특히 바퀴 기반 Odometry는 다음 문제를 가집니다.

 

바퀴 미끄러짐	바퀴는 돌았지만 실제 로봇은 덜 움직임
바닥 상태	카펫, 먼지, 경사면에서 오차 증가
바퀴 반지름 오차	실제 바퀴 크기와 파라미터가 다르면 거리 오차 발생
바퀴 간격 오차	회전 각도 계산이 틀어짐
누적 오차	작은 오차가 계속 쌓임

 

그래서 Odometry는 “현재 위치를 정확히 아는 절대 센서”가 아닙니다. 더 정확히 말하면:

 

Odometry는 짧은 시간 동안은 쓸 만하지만,
시간이 길어질수록 오차가 누적되는 상대 위치 추정이다.

 

 

따라서 실제 로봇에서는 보통 다음과 같이 조합합니다.

Odometry + IMU + LiDAR SLAM
Odometry + IMU + GPS
Odometry + UWB
Odometry + Visual SLAM