00001 #!/usr/bin/env python 00002 import rospy 00003 from math import pi 00004 from ackermann_msgs.msg import AckermannDrive 00005 from sensor_msgs.msg import Joy 00006 00007 class TeleopAckermannJoy(): 00008 def __init__(self): 00009 self.enable_button = 4 00010 self.is_enable_button = False 00011 self.axis_dead_zone = 0.05 00012 self.axis_linear = 1 00013 self.scale_linear = 1 00014 self.axis_front_steering = 3 00015 self.scale_front_steering = pi/10.0 00016 00017 rospy.Subscriber("joy", Joy, self.callback) 00018 self.pub = rospy.Publisher('cmd_ackermann', AckermannDrive, queue_size=10) 00019 00020 rospy.spin() 00021 00022 def callback(self, data): 00023 #rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.buttons[self.enable_button]) 00024 if data.buttons[self.enable_button] is 1: 00025 ackermann_msg = AckermannDrive() 00026 00027 if abs(data.axes[self.axis_linear]) > self.axis_dead_zone: 00028 ackermann_msg.speed = data.axes[self.axis_linear]*self.scale_linear #left up-down stick 00029 else: 00030 ackermann_msg.speed = 0.0 00031 00032 if abs(data.axes[self.axis_front_steering]) > self.axis_dead_zone: 00033 ackermann_msg.steering_angle = data.axes[self.axis_front_steering]*self.scale_front_steering #right left-right stick 00034 else: 00035 ackermann_msg.steering_angle = 0.0 00036 00037 self.pub.publish(ackermann_msg) 00038 00039 00040 if __name__ == '__main__': 00041 rospy.init_node('teleop_ackermann_joy', anonymous=False) 00042 try: 00043 teleop_ackermann_joy = TeleopAckermannJoy() 00044 except rospy.ROSInterruptException: pass