#!/usr/bin/env python
import roslib
import rospy

from nav_msgs.msg import Odometry
import csv

def odometryCb(msg):
    filename=r'odom_data_burger.csv'
    t=msg.header.stamp
    x_pos=msg.pose.pose.position.x
    y_pos=msg.pose.pose.position.y
    z_pos=msg.pose.pose.position.z

    x_or=msg.pose.pose.orientation.x
    y_or=msg.pose.pose.orientation.y
    z_or=msg.pose.pose.orientation.z
    w_or=msg.pose.pose.orientation.w
    
    x_vel=msg.twist.twist.angular.x
    y_vel=msg.twist.twist.angular.y
    z_vel=msg.twist.twist.angular.z

    x_vel_or=msg.twist.twist.linear.x
    y_vel_or=msg.twist.twist.linear.y
    z_vel_or=msg.twist.twist.linear.z
    
    with open(filename,mode='a') as csv_file:
        writer=csv.writer(csv_file,delimiter=',')
        writer.writerow([t, x_pos, y_pos, z_pos, x_or, y_or,z_or, w_or, 
                         x_vel, y_vel, z_vel, x_vel_or, y_vel_or, z_vel_or])
                         
    print(msg)

if __name__ == "__main__":
    rospy.init_node('oodometry', anonymous=True) #make node 
    rospy.Subscriber('/odom',Odometry,odometryCb)
    rospy.spin()
