00001 #!/usr/bin/env python 00002 import rospy 00003 from math import pi 00004 from four_wheel_steering_msgs.msg import FourWheelSteering 00005 from sensor_msgs.msg import Joy 00006 00007 class TeleopFourWheelSteeringJoy(): 00008 def __init__(self): 00009 self.axis_dead_zone = 0.05 00010 self.axis_linear_forward = 5 # Right Trigger 00011 self.axis_linear_reverse = 2 # Left Trigger 00012 self.scale_linear = 1 00013 self.axis_front_steering = 0 # Right left-right stick 00014 self.axis_rear_steering = 3 # Left left-right stick 00015 self.scale_steering = pi/10.0 00016 00017 self.is_trigger_forward_init = False 00018 self.is_trigger_reverse_init = False 00019 self.last_forward_speed = 0.0 00020 self.last_reverse_speed = 0.0 00021 00022 rospy.Subscriber("joy", Joy, self.callback) 00023 self.pub = rospy.Publisher('cmd_four_wheel_steering', FourWheelSteering, queue_size=10) 00024 00025 rospy.spin() 00026 00027 def callback(self, data): 00028 four_wheel_steering_msg = FourWheelSteering() 00029 00030 #rospy.loginfo(rospy.get_caller_id() + " axes" + str(data.axes)) 00031 linear_forward = 1.0 00032 linear_reverse = 1.0 00033 if self.is_trigger_forward_init: 00034 linear_forward = data.axes[self.axis_linear_forward] 00035 elif abs(self.last_forward_speed - data.axes[self.axis_linear_forward]) > 0.01: 00036 self.is_trigger_forward_init = True 00037 else: 00038 self.last_forward_speed = data.axes[self.axis_linear_forward] 00039 00040 if self.is_trigger_reverse_init: 00041 linear_reverse = data.axes[self.axis_linear_reverse] 00042 elif abs(self.last_reverse_speed - data.axes[self.axis_linear_reverse]) > 0.01: 00043 self.is_trigger_reverse_init = True 00044 else: 00045 self.last_reverse_speed = data.axes[self.axis_linear_reverse] 00046 00047 speed = (-linear_forward + linear_reverse)/2.0 00048 rospy.loginfo(rospy.get_caller_id() + " speed %s", speed) 00049 00050 if abs(speed) > self.axis_dead_zone: 00051 four_wheel_steering_msg.speed = speed*self.scale_linear 00052 00053 if abs(data.axes[self.axis_front_steering]) > self.axis_dead_zone: 00054 four_wheel_steering_msg.front_steering_angle = data.axes[self.axis_front_steering]*self.scale_steering 00055 else: 00056 four_wheel_steering_msg.front_steering_angle = 0.0 00057 00058 if abs(data.axes[self.axis_rear_steering]) > self.axis_dead_zone: 00059 four_wheel_steering_msg.rear_steering_angle = data.axes[self.axis_rear_steering]*self.scale_steering 00060 else: 00061 four_wheel_steering_msg.rear_steering_angle = 0.0 00062 00063 self.pub.publish(four_wheel_steering_msg) 00064 00065 00066 if __name__ == '__main__': 00067 rospy.init_node('teleop_four_wheel_steering_joy', anonymous=False) 00068 try: 00069 teleop_four_wheel_steering_joy = TeleopFourWheelSteeringJoy() 00070 except rospy.ROSInterruptException: pass