save_data.py

#!/usr/bin/env python

##
## This program reads the /g500/dvl and /g500/pose topics generated by the dynamic function of UWsim on robot Girona 500 
## Save the datas from DVL into a csv file "/data/dvl.csv"
## Save the real robot position into a csv file "/data/robot.csv"
##
## Press 'p' when you want the datas to be written on these files.
##


import rospy
from underwater_sensor_msgs.msg import DVL
from geometry_msgs.msg import Pose
import matplotlib.pyplot as plt
import numpy as np
import math
import termios, fcntl, sys, os
import csv

class dvl:
	def __init__(self):
		#Console input variables to teleop it from the console
		fd = sys.stdin.fileno()
		oldterm = termios.tcgetattr(fd)
		newattr = termios.tcgetattr(fd)
		newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
		termios.tcsetattr(fd, termios.TCSANOW, newattr)
		oldflags = fcntl.fcntl(fd, fcntl.F_GETFL)
		fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK)

		# Subscribe to the topics
		sub_dvl = rospy.Subscriber('/g500/dvl', DVL, self.dvl_sub)
		sub_pose = rospy.Subscriber('/g500/pose',Pose, self.pose_sub)

		self.timeDVL = [0]
		self.timeRobot = [0]

		self.dvlX = [0]
		self.dvlY = [0]
		self.dvlZ = [0]

		self.robotX = [0]
		self.robotY = [0]
		self.robotZ = [0]
		self.robotYaw = [0]

	# Callback of dvl topic -> save the datas in some lists
	def dvl_sub(self,msg):
		self.timeDVL.append(rospy.get_time())
		self.dvlX.append(msg.bi_x_axis)
		self.dvlY.append(msg.bi_y_axis)
		self.dvlZ.append(msg.bi_z_axis)
		# When 'p' is pressed, the datas are saved
		try:
			c = sys.stdin.read(1)
			print c
			if c == 'p' or c == 'P':
				self.displayDatas()
		except IOError: pass

	# Callback of pose topic -> save the datas in some lists
	def pose_sub(self,msg):
		self.timeRobot.append(rospy.get_time())
		self.robotX.append(msg.position.x)
		self.robotY.append(msg.position.y)
		self.robotZ.append(msg.position.z)
		self.robotYaw.append(self.quaternion_to_euler(msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w)[2])

	# Convert quaternion to euler angles (radians)
	def quaternion_to_euler(self,x,y,z,w):
		t0 = +2.0 * (w * x + y * z)
		t1 = +1.0 - 2.0 * (x * x + y * y)
		X = math.degrees(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
		Y = math.degrees(math.asin(t2))

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

		return X, Y, Z
	
	# Save the datas at this moment, then write them in he csv files
	def displayDatas(self):
		stimeDVL = self.timeDVL
		stimeRobot = self.timeRobot
		sdvlX = self.dvlX
		sdvlY = self.dvlY
		sdvlZ = self.dvlZ
		srobotX = self.robotX
		srobotY = self.robotY
		srobotZ = self.robotZ
		srobotYaw = self.robotYaw

		path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/dvl.csv")
		with open(path, mode='w') as csv_file:
			writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
			writer.writerow(['time','X','Y','Z'])

			for i in range(len(stimeDVL)):
				writer.writerow([stimeDVL[i], sdvlX[i], sdvlY[i], sdvlZ[i]])

		path = os.path.join(os.path.abspath(os.path.dirname(__file__)), "data/robot.csv")
		with open(path, mode='w') as csv_file:
			writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
			writer.writerow(['time','X','Y','Z','Yaw'])

			for i in range(len(stimeRobot)):
				writer.writerow([stimeRobot[i], srobotX[i], srobotY[i], srobotZ[i], srobotYaw[i]])		


def main(args):
	rospy.init_node('read_dvl', anonymous=True)

	start = dvl()

	try:
		rospy.spin()
	except KeyboardInterrupt:
		print("Shutting down")
		cv2.destroyAllWindows()

if __name__ == '__main__':
	main(sys.argv)