move_base: support for srf & ir
[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                         #self.get_dist_forward()
24                         #self.get_dist_backward()
25                         #self.get_dist_left()
26                         #self.get_dist_right()
27                         rate.sleep()
28
29         def set_speed(self, left, right):
30                 if left > 0: left+=80
31                 elif left < 0: left-=80
32                 if right > 0: right+=80
33                 elif right < 0: right-=80
34
35                 if left > 255: left=255
36                 elif left < -255: left=-255
37                 if right > 255: right=255
38                 elif right < -255: right=-255
39
40                 dev = i2c(0x56)
41                 s = struct.pack(">Bhh", 0x1, left, right)
42                 dev.write(s)
43                 dev.close()
44
45         def cmdVelReceived(self, msg):
46                 trans = msg.linear.x
47                 rot = msg.angular.z # rad/s
48
49                 right = rot*pi*WHEEL_DIST + trans
50                 left = trans*2-right
51                 self.set_speed(left, right)
52
53         # http://rn-wissen.de/wiki/index.php/Sensorarten#Sharp_GP2D12
54         def get_dist_ir(self, num):
55                 dev = i2c(0x52)
56                 s = struct.pack("B", num)
57                 dev.write(s)
58                 dev.close()
59
60                 sleep(2e-6)
61
62                 dev = i2c(0x52)
63                 s = dev.read(2)
64                 dev.close()
65
66                 val = struct.unpack(">H", s)[0]
67                 return 15221/(val - -276.42)/100;
68         
69         def get_dist_srf(self, num):
70                 dev = i2c(0x52)
71                 s = struct.pack("B", num)
72                 dev.write(s)
73                 dev.close()
74
75                 sleep(50e-3)
76
77                 dev = i2c(0x52)
78                 s = dev.read(2)
79                 dev.close()
80
81                 return struct.unpack(">H", s)[0]/1000.0
82
83         def get_dist_left(self):
84                 dist = self.get_dist_ir(0x1)
85
86         def get_dist_right(self):
87                  dist = self.get_dist_ir(0x3)
88
89         def get_dist_forward(self):
90                 dist = self.get_dist_srf(0x5)
91
92         def get_dist_backward(self):
93                 dist = self.get_dist_srf(0x7)
94
95
96 if __name__ == "__main__":
97         MoveBase()