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