print_odom

#! /usr/bin/env python
import rospy

from nav_msgs.msg import Odometry
from math import atan2

if __name__=='__main__':
	rospy.init_node('print_odom')

	odom_info = rospy.wait_for_message('/tb2/odom', Odometry)

	x = odom_info.pose.pose.position.x
	y = odom_info.pose.pose.position.y
	theta = 2 * atan2(odom_info.pose.pose.orientation.z, odom_info.pose.pose.orientation.w)

	print("x: %.2f / y: %.2f / theta: %.2f"%(x, y, theta))