[ROS] odom과 gt를 수신하여 텍스트 파일에 적어보자

비동기화된 odom 데이터와 gt 데이터를 어떤 식으로 비교할까 ? ?

개요

비동기화된 odom 데이터와 gt 데이터가 있다.
각 메세지 헤더에 저장된 timestamp 정보로 유사하게 동기화 시켜서 에러를 비교할 순 있다.
완전 정확한 동기화는 아직까지는 내 수준에서는 잘 모르겠다.

따라서 위 방법을 간략히 요약하고자 한다.
모든 방법은 ros기반 파이썬 3에서 코딩되었다.

메인 코드

#!/usr/env/bin python3

import rospy
from nav_msgs.msg import Odometry
import queue
from dataclasses import dataclass
import math
 
def euler_from_quaternion(x, y, z, w):
  t0 = +2.0 * (w * x + y * z)
  t1 = +1.0 - 2.0 * (x * x + y * y)
  roll_x = math.atan2(t0, t1)

  t2 = +2.0 * (w * y - z * x)
  t2 = +1.0 if t2 > +1.0 else t2
  t2 = -1.0 if t2 < -1.0 else t2
  pitch_y = math.asin(t2)

  t3 = +2.0 * (w * z + x * y)
  t4 = +1.0 - 2.0 * (y * y + z * z)
  yaw_z = math.atan2(t3, t4)

  return roll_x, pitch_y, yaw_z # in radians

@dataclass
class pose:
  seq: int = 0
  timestamp: float = 0.0
  x: float = 0.0
  y: float = 0.0
  theta: float = 0.0

odom_queue = queue.Queue()
gt_queue = queue.Queue()

odom_list = []
gt_list = []
odom_pose_list = []
gt_pose_list = []

def gtCallback(msg):
  gt_queue.put(msg)

def odomCallback(msg):
  odom_queue.put(msg)

odom_sub = rospy.Subscriber('/odom', Odometry, odomCallback, queue_size=1000)
gt_sub = rospy.Subscriber('/gt', Odometry, gtCallback, queue_size=1000)

odom_seq = 0
gt_seq = 0

def print_pose(pos):
  print(' seq : ', pos.seq)
  print('time : ', pos.timestamp)
  print('  y  : ', pos.x)
  print('  z  : ', pos.y)
  print('theta: ', pos.theta*180/math.pi)

def process():
  global odom_seq, gt_seq
  # print('\n\n============')
  is_end = False
  
  if gt_queue.qsize() > 0:
    gt_list.append(gt_queue.get())
    
  if odom_queue.qsize() > 0:
    odom_list.append(odom_queue.get())
  
  '''
  For odom
  '''
  odom_idx = len(odom_list) - 1
  if len(odom_list) > 0:
    pose_msg = pose()
    pose_msg.seq = odom_seq
    pose_msg.timestamp = float(odom_list[odom_idx].header.stamp.secs) \
                       + float( 1e-3*(int(odom_list[odom_idx].header.stamp.nsecs/(1e6))) )
    pose_msg.x = odom_list[odom_idx].pose.pose.position.x
    pose_msg.y = odom_list[odom_idx].pose.pose.position.y
    r, p, y = euler_from_quaternion(odom_list[odom_idx].pose.pose.orientation.x,\
                                    odom_list[odom_idx].pose.pose.orientation.y,\
                                    odom_list[odom_idx].pose.pose.orientation.z,\
                                    odom_list[odom_idx].pose.pose.orientation.w,)
    pose_msg.theta = y
    
    odom_seq += 1
    odom_pose_list.append(pose_msg)
    del odom_list[0]
    
  '''
  For Ground Truth Pose
  '''
  gt_idx = len(gt_list) - 1
  if len(gt_list) > 0:
    pose_msg = pose()
    pose_msg.seq = gt_seq
    pose_msg.timestamp = float(gt_list[gt_idx].header.stamp.secs) \
                       + float( 1e-3*(int(gt_list[gt_idx].header.stamp.nsecs/(1e6))) )
    pose_msg.x = gt_list[gt_idx].pose.pose.position.x
    pose_msg.y = gt_list[gt_idx].pose.pose.position.y
    r, p, y = euler_from_quaternion(gt_list[gt_idx].pose.pose.orientation.x,\
                                    gt_list[gt_idx].pose.pose.orientation.y,\
                                    gt_list[gt_idx].pose.pose.orientation.z,\
                                    gt_list[gt_idx].pose.pose.orientation.w,)
    pose_msg.theta = y
    
    gt_seq += 1
    gt_pose_list.append(pose_msg)
    del gt_list[0]
    
    print_pose(pose_msg)
    if (pose_msg.timestamp > 621): is_end = True

  if is_end: return is_end
  
if __name__== '__main__':
  rospy.init_node('odom_gt_write_node', anonymous=True)

  while not rospy.is_shutdown():
    end_condition = process()
    
    if end_condition: break 
  
  print('\n\n==================================================')
  print('=========              END            ============')
  print('==================================================\n\n')
  
  print('gt_pose.size: ', len(gt_pose_list))
  for gt_pose in gt_pose_list:
    pass
    
  print('odom.size: ', len(odom_pose_list))
  for odom_pose in odom_pose_list:
    pass
    
  # Write text file
  f_gt = open("gt.txt", 'w')
  for gt_pose in gt_pose_list:
    line = str(gt_pose.seq) + "," + str(gt_pose.timestamp) + "," + str(gt_pose.x) + "," + str(gt_pose.y) + "," + str(gt_pose.theta) + "\n"
    f_gt.write(line)
  
  f_gt.close()
  
  f_odom = open("odom.txt", 'w')
  for odom_pose in odom_pose_list:
    line = str(odom_pose.seq) + "," + str(odom_pose.timestamp) + "," + str(odom_pose.x) + "," + str(odom_pose.y) + "," + str(odom_pose.theta) + "\n"
    f_odom.write(line)
  
  f_odom.close()

설명

1. odom, gt 데이터를 각각 queue에 밀어넣는다. 참고로 파이썬에는 알아서 공유 자원에 대한 lock, unlock을 해주나 보다?
2. pose라는 구조체를 정의해 내가 원하는 데이터만 ros 메세지에서 저장한다
3. 메세지 header에 저장된 시간이 621초가 지나면 데이터 수신 단계를 종료한다.
4. 지금껏 저장된 pose를 이용하여 각각 gt.txt, odom.txt에 저장한다.
5. 매트랩으로 비교할것이다 !