Odometry Basic

Odometetor (오도미터)

차량이나 로봇이 주행하며 이동한 거리를 측정하는 기기
바퀴의 회전수로 이동거리를 계산함

Odometetry (오도메트리)

오도미터 등의 기기의 측정값으로 움직이는 사물의 위치를 측정하는 방법

Ackermann Steering (스티어링 기하학 )

안쪽과 바깥쪽 회전 원의 중심이 일치해야 하므로
회전시 꺾이는 각도가 다름

https://en.wikipedia.org/wiki/Ackermann_steering_geometry

Odometry topic

• topic: /odom
• msgtype: nav_msgs/Odometry

$ rosmsg show nav_msgs/Odometry

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose 
    geometry_msgs/Point position # 3차원 공간 좌표
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation # roll, yaw, picth 기울기 정보의 Quaternion 표기법
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear # 선속도
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular # 긱속도
      float64 x
      float64 y
      float64 z
  float64[36] covariance

Odometry Publisher

#!/usr/bin/env python3

import math
from math import sin, cos, pi

import rospy
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3

rospy.init_node('odometry_publisher')

odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()

x = 0.0
y = 0.0
th = 0.0

vx = 0.1
vy = -0.1
vth = 0.1

current_time = rospy.Time.now()
last_time = rospy.Time.now()

r = rospy.Rate(1.0)
while not rospy.is_shutdown():
    current_time = rospy.Time.now()

    # compute odometry in a typical way given the velocities of the robot
    dt = (current_time - last_time).to_sec()
    delta_x = (vx * cos(th) - vy * sin(th)) * dt
    delta_y = (vx * sin(th) + vy * cos(th)) * dt
    delta_th = vth * dt

    x += delta_x
    y += delta_y
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()