drone-rigide/scripts/untwist.py

32 lines
1.2 KiB
Python
Raw Normal View History

2019-06-02 16:02:15 +00:00
#! /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()