#! /usr/bin/python
import rospy

from geometry_msgs.msg import Twist
from std_msgs.msg import Float64


class UnTwister:

    def __init__(self):
        self.twist = Twist()
        self.linear_x = rospy.Publisher('measure_linear_x', Float64, queue_size=1)
        self.linear_y = rospy.Publisher('measure_linear_y', Float64, queue_size=1)
        self.linear_z = rospy.Publisher('measure_linear_z', Float64, queue_size=1)
        self.angular_x = rospy.Publisher('measure_angular_x', Float64, queue_size=1)
        self.angular_y = rospy.Publisher('measure_angular_y', Float64, queue_size=1)
        self.angular_z = rospy.Publisher('measure_angular_z', Float64, queue_size=1)
        self.input = rospy.Subscriber('input', Twist, self.on_compute, queue_size=1)

    def on_compute(self, twist):
        self.linear_x.publish(twist.linear.x)
        self.linear_y.publish(twist.linear.y)
        self.linear_z.publish(twist.linear.z)
        self.angular_x.publish(twist.angular.x)
        self.angular_y.publish(twist.angular.y)
        self.angular_z.publish(twist.angular.z)

if __name__ == '__main__':
    rospy.init_node('untwist')
    twister = UnTwister()
    rospy.spin()