#!/usr/bin/env python

# Copyright (C) 2017 Udacity Inc.
# All Rights Reserved.

# Author: Brandon Kinman

import rospy
import tf

from geometry_msgs.msg import PoseStamped
from quad_controller.msg import EulerAngles
from sensor_msgs.msg import Imu

class QuaternionToEuler():
    def __init__(self):
        self.imu_sub_   = rospy.Subscriber("/quad_rotor/imu", Imu, self.imu_callback)
        self.pose_sub_  = rospy.Subscriber("/quad_rotor/pose", PoseStamped, self.pose_callback)
        self.imu_euler_pub_ = rospy.Publisher("/quad_rotor/imu_euler_angles", EulerAngles, queue_size = 1)
        self.pose_euler_pub_ = rospy.Publisher("/quad_rotor/pose_euler_angles", EulerAngles, queue_size = 1)

        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            rate.sleep()

    def imu_callback(self, msg):
        ea_msg = EulerAngles()

        ea_msg.header = msg.header

        (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([msg.orientation.x,
                                                                        msg.orientation.y,
                                                                        msg.orientation.z,
                                                                        msg.orientation.w])
        ea_msg.roll = roll
        ea_msg.pitch = pitch
        ea_msg.yaw = yaw
        self.imu_euler_pub_.publish(ea_msg)

    def pose_callback(self, msg):
        ea_msg = EulerAngles()

        ea_msg.header = msg.header

        (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([msg.pose.orientation.x,
                                                                        msg.pose.orientation.y,
                                                                        msg.pose.orientation.z,
                                                                        msg.pose.orientation.w])
        ea_msg.roll = roll
        ea_msg.pitch = pitch
        ea_msg.yaw = yaw
        self.pose_euler_pub_.publish(ea_msg)

if __name__ == '__main__':
    rospy.init_node('quaternion_to_euler')

    try:
        q2e = QuaternionToEuler()
    except rospy.ROSInterruptException:
        pass
