32 lines
1.2 KiB
Python
32 lines
1.2 KiB
Python
|
#! /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()
|