#!/usr/bin/env python3

import rospy 
import rospkg 
import csv
from gazebo_msgs.msg import ModelState 
from gazebo_msgs.srv import SetModelState

def main():
	rospy.init_node('set_pose')
	
	state_msg = ModelState()
	state_msg.model_name = 'my_submarine'
	
	state_file = open('state.csv', newline='')
	reader = csv.DictReader(state_file)
	
	for row in reader:
		state_msg.pose.position.x = float(row['px'])
		state_msg.pose.position.y = float(row['py'])
		state_msg.pose.position.z = float(row['pz'])
		state_msg.pose.orientation.w = float(row['q1'])
		state_msg.pose.orientation.x = float(row['q2'])
		state_msg.pose.orientation.y = float(row['q3'])
		state_msg.pose.orientation.z = float(row['q4'])
		
		rospy.wait_for_service('/gazebo/set_model_state')
		try:
			set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
			resp = set_state( state_msg )
		except rospy.ServiceException as e:
			print("Service call failed: %s" % e)
		rospy.sleep(0.1)

if __name__ == '__main__':
	try:
		main()
	except rospy.ROSInterruptException:
		pass
