added teleop, move_base for cmd_vel
[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 > 255: left=255
27                 elif left < -255: left=-255
28                 if right > 255: right=255
29                 elif right < -255: right=-255
30
31                 dev = i2c(0x56)
32                 s = struct.pack(">Bhh", 0x1, left, right)
33                 dev.write(s)
34                 dev.close()
35
36         def cmdVelReceived(self, msg):
37                 trans = msg.linear.x
38                 rot = msg.angular.z # rad/s
39
40                 right = rot*pi*WHEEL_DIST + trans
41                 left = trans*2-right
42                 self.set_speed(left, right)
43
44
45 if __name__ == "__main__":
46         MoveBase()