#!/usr/bin/python

import smbus
import time
import math
import numpy
import rospy
from rospy import Time
from geometry_msgs.msg import Quaternion, Twist
from tf.broadcaster import TransformBroadcaster
from sensor_msgs.msg import Imu
from project_ros_python.msg import get_degree

import tf
from tf.transformations import *

MY_PI = 3.14159265358979323846

def callback(data):

	roll = data.roll
	pitch = data.pitch
	yaw = data.yaw
	acc_x = data.acc_x
	acc_y = data.acc_y
	acc_z = data.acc_z
 	
	q_rot = quaternion_from_euler(roll*MY_PI/180, pitch*MY_PI/180, yaw*MY_PI/180)
		
	#print ("roll:%.3f, pitch:%.3f, yaw:%.6f" % (roll,pitch,yaw))

	imu_msg = Imu()

	imu_msg.linear_acceleration.x = 0
	imu_msg.linear_acceleration.y = 0
	imu_msg.linear_acceleration.z = 9.80665

	imu_msg.angular_velocity.x = 0.0
	imu_msg.angular_velocity.y = 0.0
	imu_msg.angular_velocity.z = 0.0

	imu_msg.orientation.x = q_rot[0]
	imu_msg.orientation.y = q_rot[1]
	imu_msg.orientation.z = q_rot[2]
	imu_msg.orientation.w = q_rot[3]

	imu_msg.header.stamp = rospy.Time.now()
	imu_msg.header.frame_id = frame_name


	odomBroadcaster_imu = TransformBroadcaster()
 	odomBroadcaster_imu.sendTransform(
		(0, 0, 0),
		(0,0,0,1),
		rospy.Time.now(), frame_name, fixed_frame)


	pub.publish(imu_msg)
	#print("imu pub success")


if __name__ == "__main__":

	rospy.init_node('my_publisher_imu', anonymous=True)

	fixed_frame = rospy.get_param('~fixed_frame', "base_footprint")
	frame_name = rospy.get_param('~frame_name', "imu")


	rospy.Subscriber('project_degree', get_degree, callback)
	pub = rospy.Publisher('/imu', Imu, queue_size=1)
    
	rospy.spin()