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