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()