drone-rigide/scripts/twist_controls.py

38 lines
1.3 KiB
Python
Raw Normal View History

2019-05-24 11:13:28 +00:00
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
class Twister:
def __init__(self):
self.twist = Twist()
self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.control_linear_z = rospy.Subscriber('control_linear_z', Float64, self.on_linear_z, queue_size=1)
self.control_angular_z = rospy.Subscriber('control_angular_z', Float64, self.on_angular_z, queue_size=1)
self.control_linear_x = rospy.Subscriber('control_linear_x', Float64, self.on_linear_x, queue_size=1)
self.control_linear_y = rospy.Subscriber('control_linear_y', Float64, self.on_linear_y, queue_size=1)
def on_linear_x(self, value):
self.twist.linear.x = value.data
2019-06-02 16:02:15 +00:00
self.twist_pub.publish(self.twist)
2019-05-24 11:13:28 +00:00
def on_linear_y(self, value):
self.twist.linear.y = value.data
2019-06-02 16:02:15 +00:00
self.twist_pub.publish(self.twist)
2019-05-24 11:13:28 +00:00
def on_linear_z(self, value):
self.twist.linear.z = value.data
2019-06-02 16:02:15 +00:00
self.twist_pub.publish(self.twist)
2019-05-24 11:13:28 +00:00
def on_angular_z(self, value):
self.twist.angular.z = value.data
2019-06-02 16:02:15 +00:00
self.twist_pub.publish(self.twist)
2019-05-24 11:13:28 +00:00
if __name__ == '__main__':
rospy.init_node('twist_controls')
twister = Twister()
rospy.spin()