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

from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState
import csv

def odometryCb(msg):
    filename=r'odom_data.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)

def joint_stateCb(msg):
    filename=r'joint_state_data.csv'
    t=msg.header.stamp
    gripper_pos=msg.position[0]
    gripper_vel=msg.velocity[0]
    gripper_effort=msg.effort[0]
    
    gripper_sub_pos=msg.position[1]
    gripper_sub_vel=msg.velocity[1]
    gripper_sub_effort=msg.effort[1]
    
    joint1_pos=msg.position[2]
    joint1_vel=msg.velocity[2]
    joint1_effort=msg.effort[2]
    
    joint2_pos=msg.position[3]
    joint2_vel=msg.velocity[3]
    joint2_effort=msg.effort[3]
    
    joint3_pos=msg.position[4]
    joint3_vel=msg.velocity[4]
    joint3_effort=msg.effort[4]    
    
    joint4_pos=msg.position[5]
    joint4_vel=msg.velocity[5]
    joint4_effort=msg.effort[5]
        
    with open(filename,mode='a') as csv_file:
        writer=csv.writer(csv_file,delimiter=',')
        writer.writerow([t, gripper_pos, gripper_sub_pos,
                         joint1_pos, joint2_pos,joint3_pos,joint4_pos,
                         gripper_vel, gripper_sub_vel,
                         joint1_vel, joint2_vel,joint3_vel,joint4_vel,
                         gripper_effort, gripper_sub_effort,
                         joint1_effort, joint2_effort,joint3_effort,joint4_effort])
   
    print(msg)

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