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