robot@robot:~/robot_ws$ ros2 interface show nav_msgs/msg/Odometry
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
# Includes the frame id of the pose parent.
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# Frame id the pose points to. The twist is in this coordinate frame.
string child_frame_id
# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[36] covariance
# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
Twist twist
Vector3 linear
float64 x
float64 y
float64 z
Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
nav_msgs/msg/Odometry 데이터는 로봇의 "어디에 있는가(Pose)"와 "어떻게 움직이는가(Twist)"를 모두 포함하는 종합 선물 세트다.
1. Header (데이터의 신분증)
• stamp: 이 데이터가 측정된 정확한 시각.
• frame_id: 기준이 되는 지도(월드) 좌표계. 보통 odom이나 map이 들어간다.
• child_frame_id: 로봇 본체 좌표계. 보통 base_footprint나 base_link
2. PoseWithCovariance (로봇의 현재 위치)
"로봇이 기준 좌표계(frame_id)로부터 얼마나 떨어져 있는가"를 나타낸다.
• pose (위치와 방향):
◦ position (x, y, z): 좌표 상의 위치. 평면 이동을 하는 터틀봇은 주로 x와 y만 사용.
◦ orientation (x, y, z, w): 로봇이 바라보는 방향입니다. 일반적인 각도(0~360도)가 아닌 **쿼터니언(Quaternion)**이라는 사원수 체계를 사용합니다. (나중에 이를 각도로 바꾸는 변환 과정이 필요합니다.)
• covariance (신뢰도): 36개의 숫자로 된 행렬. 센서 오차를 고려했을 때 "이 위치 값이 얼마나 정확한가"를 통계적으로 나타낸다.
3. TwistWithCovariance (로봇의 현재 속도)
"로봇 본체 기준(child_frame_id)으로 지금 얼마나 빠르게 움직이는가"를 나타낸다.
• twist (선속도와 각속도):
◦ linear (x, y, z): 진행 방향 속도입니다. linear.x는 앞뒤 속도.
◦ angular (x, y, z): 회전 속도입니다. 터틀봇은 제자리 회전인 angular.z를 주로 사용.
• covariance (신뢰도): 속도 측정값이 얼마나 정확한지 나타내는 행렬.
💡 한 눈에 이해하는 포인트
| 항목 | 질문 | 데이터 접근 예시 (Python) |
|---|---|---|
| Pose | "지금 어디야?" | msg.pose.pose.position.x |
| Orientation | "어디 보고 있어?" | msg.pose.pose.orientation.w (쿼터니언) |
| Twist | "얼마나 빨라?" | msg.twist.twist.linear.x |
import math
import os
import sys
import termios
from nav_msgs.msg import Odometry
import numpy
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
class TurtlebotPose(Node):
def __init__(self):
super().__init__('move_pose_turtle')
self.odom = Odometry()
self.last_pose_x = 0.0
self.last_pose_y = 0.0
self.last_pose_theta = 0.0
self.goal_pose_x = 0.0
self.goal_pose_y = 0.0
self.goal_pose_theta = 0.0
self.init_odom_state = False
self.count = 0
qos = QoSProfile(depth=10)
self.odom_sub = self.create_subscription(
Odometry,
'odom',
self.odom_callback,
qos)
def odom_callback(self, msg):
self.last_pose_x = msg.pose.pose.position.x
self.last_pose_y = msg.pose.pose.position.y
_, _, self.last_pose_theta = self.euler_from_quaternion(msg.pose.pose.orientation)
self.init_odom_state = True
if self.count > 20:
self.get_logger().info(f'last_pose_x: {msg.pose.pose.position.x} last_pose_y = {self.last_pose_y }')
self.get_logger().info(f'last_pose_theta: {self.last_pose_theta}')
self.count = 0
self.count += 1
def euler_from_quaternion(self, quat):
x = quat.x
y = quat.y
z = quat.z
w = quat.w
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = numpy.arctan2(sinr_cosp, cosr_cosp)
sinp = 2 * (w * y - z * x)
pitch = numpy.arcsin(sinp)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = numpy.arctan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def main(args=None):
rclpy.init(args=args)
node = TurtlebotPose()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
윗 노트 프로그램을 분석해서, 터틀을 움직이는 것은 move_turtle_logic.py에서 담당하도록 하고, 터틀의 위치 및 자세에 관련된 것은 turtle_pose_and_position.py에서 담당하도록 한다.
아래는 터틀의 위치와 방향을 추가한 GUI 화면.