collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Joseph RyanMerge updated motor codec9fbf70

imu
1.9 KiB61 linesraw
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
14    Defaults to arcade drive with optional tank drive"""
15
16    if tank:
17        return (joy.leftY(), joy.rightY())
18    x = joy.leftX()
19    y = joy.leftY()
20    #TODO: test/fix arcade drive and motor directions
21    return ((y + x) / 2, (y - x) / 2)
22
23
24def publisher():
25    """Publishes /left and /right based on joystick input
26    The left and right triggers are treated as enables for the respective motors"""
27
28    rospy.init_node('manual_control', log_level=rospy.INFO)
29    joystick = None
30    for _ in range(3):
31        try:
32            joystick = xbox.Joystick()
33        except IOError:
34            rospy.logerr(
35                "Failed to connect to xbox controller. Trying agin in 10 seconds..."
36            )
37            rospy.sleep(10.)
38    if not joystick:
39        rospy.logfatal("Xbox controller not found. Shutting down...")
40        rospy.signal_shutdown("Couldn't connect to xbox controller")
41    rospy.loginfo("Joystick found")
42    #TODO measure speed going straght and constrain to 5mph
43    max_speed = .5
44    left = rospy.Publisher('left', Float32, queue_size=10)
45    right = rospy.Publisher('right', Float32, queue_size=10)
46    #TODO: test other rates and find a balance between bandwidth usage and fine control
47    rate = rospy.Rate(10)
48    while not rospy.is_shutdown():
49        left_speed, right_speed = joy2speed(joystick, True)
50        left_speed *= max_speed * joystick.leftTrigger()
51        right_speed *= max_speed * joystick.rightTrigger()
52        left.publish(left_speed)
53        right.publish(right_speed)
54        rate.sleep()
55
56
57if __name__ == '__main__':
58    try:
59        publisher()
60    except rospy.ROSInterruptException:
61        pass