#!/usr/bin/env python

# Copyright(c) 2017, SCUT RIS
# author : Jinhui Zhu

import rospy

from nav_msgs.msg import Odometry


def odom_callback(data):
	rx = data.pose.pose.position.x;
	ry = data.pose.pose.position.y;
	rz = data.pose.pose.position.z;
	rospy.loginfo(rospy.get_caller_id()+": %.2f %.2f %.2f",rx,ry,rz);
	x = data.pose.pose.orientation.x;
	y = data.pose.pose.orientation.y;
	z = data.pose.pose.orientation.z;
	w = data.pose.pose.orientation.w;

	rospy.loginfo(rospy.get_caller_id()+": %.2f %.2f %.2f %.2f",x,y,z,w);


if __name__ == "__main__":
	rospy.init_node("where");

	sub = rospy.Subscriber('/odom',Odometry, odom_callback);

	rospy.spin();
