tuning
[ros_wild_thumper.git] / scripts / move_base.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import rospy
5 import struct
6 from i2c import i2c
7 from math import *
8 from geometry_msgs.msg import Twist
9
10 WHEEL_DIST = 0.248
11
12 class MoveBase:
13         def __init__(self):
14                 rospy.init_node('wild_thumper_move_base')
15                 rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
16                 self.set_speed(0, 0)
17                 rospy.loginfo("Init done")
18                 self.run()
19         
20         def run(self):
21                 rate = rospy.Rate(10.0)
22                 while not rospy.is_shutdown():
23                         rate.sleep()
24
25         def set_speed(self, left, right):
26                 if left > 0: left+=80
27                 elif left < 0: left-=80
28                 if right > 0: right+=80
29                 elif right < 0: right-=80
30
31                 if left > 255: left=255
32                 elif left < -255: left=-255
33                 if right > 255: right=255
34                 elif right < -255: right=-255
35
36                 dev = i2c(0x56)
37                 s = struct.pack(">Bhh", 0x1, left, right)
38                 dev.write(s)
39                 dev.close()
40
41         def cmdVelReceived(self, msg):
42                 trans = msg.linear.x
43                 rot = msg.angular.z # rad/s
44
45                 right = rot*pi*WHEEL_DIST + trans
46                 left = trans*2-right
47                 self.set_speed(left, right)
48
49
50 if __name__ == "__main__":
51         MoveBase()