collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
fbe4edd
documentation
1#!/usr/bin/env python 2""" Publishes motor speed topics based on joystick input 3 4Requires xboxdrv (sudo apt install xboxdrv) 5""" 6 7import xbox 8import rospy 9from std_msgs .msg import Float32 10 11 12def joy2speed (joy ,tank = False ): 13"""Converts joystick x and y axis values to left and right motor speeds 14Defaults to arcade drive with optional tank drive""" 15 16if tank : 17return (joy .leftY (),joy .rightY ()) 18x = joy .leftX () 19y = joy .leftY () 20#TODO: test/fix arcade drive and motor directions 21return ((y + x )/ 2 , (y - x )/ 2 ) 22 23 24def publisher (): 25"""Publishes /left and /right based on joystick input 26The left and right triggers are treated as enables for the respective motors""" 27 28rospy .init_node ('manual_control' ,log_level = rospy .INFO ) 29joystick = None 30for _ in range (3 ): 31try : 32joystick = xbox .Joystick () 33except IOError : 34rospy .logerr ( 35"Failed to connect to xbox controller. Trying agin in 10 seconds..." 36 ) 37rospy .sleep (10. ) 38if not joystick : 39rospy .logfatal ("Xbox controller not found. Shutting down..." ) 40rospy .signal_shutdown ("Couldn't connect to xbox controller" ) 41rospy .loginfo ("Joystick found" ) 42#TODO measure speed going straght and constrain to 5mph 43max_speed = .5 44left = rospy .Publisher ('left' ,Float32 ,queue_size = 10 ) 45right = rospy .Publisher ('right' ,Float32 ,queue_size = 10 ) 46#TODO: test other rates and find a balance between bandwidth usage and fine control 47rate = rospy .Rate (10 ) 48while not rospy .is_shutdown (): 49left_speed ,right_speed = joy2speed (joystick ,True ) 50#TODO: check trigger and joystick max values 51left_speed *= max_speed * joystick .leftTrigger () 52right_speed *= max_speed * joystick .rightTrigger () 53left .publish (left_speed ) 54right .publish (right_speed ) 55rate .sleep () 56 57 58if __name__ == '__main__' : 59try : 60publisher () 61except rospy .ROSInterruptException : 62pass