added teleop, move_base for cmd_vel
authorErik Andresen <erik@vontaene.de>
Sat, 4 Apr 2015 18:20:29 +0000 (20:20 +0200)
committerErik Andresen <erik@vontaene.de>
Sat, 4 Apr 2015 18:20:29 +0000 (20:20 +0200)
launch/teleop.launch [new file with mode: 0644]
scripts/bootloader.py [new file with mode: 0755]
scripts/i2c.py [new file with mode: 0755]
scripts/move_base.py [new file with mode: 0755]

diff --git a/launch/teleop.launch b/launch/teleop.launch
new file mode 100644 (file)
index 0000000..60f4adf
--- /dev/null
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+       <node pkg="turtlebot_teleop" type="turtlebot_teleop_joy" name="turtlebot_teleop_joystick">
+               <param name="scale_linear" value="35"/>
+               <param name="scale_angular" value="25"/>
+               <param name="axis_deadman" value="10"/>
+               <param name="axis_linear" value="1"/>
+               <param name="axis_angular" value="0"/>
+               <remap from="turtlebot_teleop_joystick/cmd_vel" to="/cmd_vel"/>
+       </node>
+
+       <node pkg="wiimote" type="wiimote_node.py" name="wiimote" respawn="true" />
+</launch>
diff --git a/scripts/bootloader.py b/scripts/bootloader.py
new file mode 100755 (executable)
index 0000000..2abcb93
--- /dev/null
@@ -0,0 +1,179 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import struct
+import sys
+import socket
+from optparse import OptionParser
+from time import sleep, time
+from i2c import i2c
+
+CMD_READ = 0x1
+CMD_ERASE = 0x2
+CMD_WRITE = 0x3
+CMD_ERASE_ALL = 0x5
+CMD_JUMP = 0x6
+CMD_INFO = 0x99
+
+PAGESIZE=64
+
+class bootloader:
+       def __init__(self, addr):
+               self.i2c_addr = addr    
+               self.boot_addr = 0x0
+
+               if not self.identify():
+                       raise "Bootloader not running"
+       
+       def read_mem(self, addr, num):
+               self.run_cmd(CMD_READ, addr, num)
+               return self.read(num)
+
+       def erase(self, addr):
+               self.run_cmd(CMD_ERASE)
+       
+       def erase_all(self):
+               self.run_cmd(CMD_ERASE_ALL)
+
+       def __compare_memarea(self, addr, data):
+                       mem_cmp = self.read_mem(addr, 64)
+                       if mem_cmp != data:
+                               print "Expected:", data.encode("hex")
+                               print "Got:     ", mem_cmp.encode("hex")
+                               raise("Compare mismatch at 0x%x" % addr)
+                       return
+       
+       def __program_memarea(self, addr, data):
+               self.run_cmd(CMD_WRITE, addr, 64, data)
+
+       def compare(self, filename):
+               return self.__process_hex(filename, self.__compare_memarea)
+       
+       def program(self, filename):
+               return self.__process_hex(filename, self.__program_memarea)
+
+       def __process_hex(self, filename, handle):
+               next_addr = None
+               buf = ""
+               lFirstRow = None
+
+               f = open(filename, "r")
+               count=0
+               for line in f:
+                       if line[0] != ':':
+                               raise("Bad line start character")
+                       hex = line[1:].replace("\r\n", "")
+                       data = hex.decode("hex")
+                       num = ord(data[0])
+                       chksum = 0
+                       for c in data:
+                               chksum+=ord(c)
+                       if chksum % 256 != 0:
+                               raise("Checksum error")
+                       addr, typ, data, chksum = struct.unpack(">HB%ssB" % num, data[1:])
+
+                       if typ == 0: # Data Record
+                               count+=len(data)
+                               if next_addr is not None:
+                                       if next_addr != addr:
+                                               raise "Gap in file"
+                               buf_addr = addr-len(buf)
+                               buf+=data
+                               if len(buf) >= PAGESIZE:
+                                       if not lFirstRow:
+                                               # do the first as last one
+                                               lFirstRow = (buf_addr, buf[:PAGESIZE])
+                                       else:
+                                               print "Addr 0x%x" % buf_addr
+                                               handle(buf_addr, buf[:PAGESIZE])
+                                       buf = buf[PAGESIZE:]
+                       elif typ == 3: # Start Segment Address Record
+                               self.boot_addr = int(data.encode("hex"), 16)
+                       elif typ == 1: # End of File Record
+                               print "Addr (rest) 0x%x" % buf_addr
+                               buf_addr+=PAGESIZE
+                               diff = PAGESIZE-len(buf)
+                               buf+=chr(0xff)*diff # fill with 0xff
+                               handle(buf_addr, buf[:PAGESIZE])
+                               if lFirstRow: # was first
+                                       buf_addr = lFirstRow[0]
+                                       buf = lFirstRow[1]
+                                       print "Addr (First) 0x%x" % buf_addr
+                                       handle(buf_addr, buf)
+                       else:
+                               raise("Unknown type %d" % typ)
+                       
+                       next_addr = addr+num
+               print "Byte count:", count
+               f.close()
+
+       def jump(self, addr):
+               self.run_cmd(CMD_JUMP)
+
+       def wait_ping(self):
+               while(True):
+                       try:
+                               self.identify()
+                               break
+                       except:
+                               sleep(1)
+
+       def load(self, filename):
+               print "Erase..."
+               self.erase_all()
+               self.wait_ping()
+               print "Erase Done."
+               print "Program..."
+               t1 = time()
+               self.program(filename)
+               print "Time: %.1fs" % (time() - t1)
+               print "Compare..."
+               t1 = time()
+               self.compare(filename)
+               print "Time: %.1fs" % (time() - t1)
+               print "Jump:"
+               self.jump(self.boot_addr)
+
+       def write(self, s):
+               dev = i2c(self.i2c_addr)
+               dev.write(s)
+               dev.close()
+
+       def read(self, num):
+               dev = i2c(self.i2c_addr)
+               s = dev.read(num)
+               dev.close()
+               return s
+       
+       def run_cmd(self, cmd, addr=0x0, num=0, data=""):
+               length = len(data)
+               s1 = struct.pack("<BLB%ds" % (length), cmd, addr, num, data)
+               self.write(s1)
+               s2 = struct.pack("B", 0xff)
+               self.write(s2)
+
+       def identify(self):
+               self.run_cmd(CMD_INFO)
+               s = self.read(10)
+               return s == "Bootloader"
+
+def to_bootloader(addr):
+       dev = i2c(addr)
+       s = struct.pack("B", 0xff)
+       dev.write(s)
+       dev.close()
+
+if __name__ == "__main__":
+       usage = "usage: %prog [options] addr [ihex]"
+       parser = OptionParser(usage=usage)
+       parser.add_option("-b", "--start-bootloader", action="store_true", dest="bToBoot", default=False, help="Start Bootloader")
+
+       (options, args) = parser.parse_args()
+       if len(args) > 1:
+               addr = int(args[0], 16)
+               if options.bToBoot:
+                       to_bootloader(addr)
+                       sleep(1)
+               if len(args) > 1:
+                       loader = bootloader(addr)
+                       loader.load(args[1])
diff --git a/scripts/i2c.py b/scripts/i2c.py
new file mode 100755 (executable)
index 0000000..2e9a109
--- /dev/null
@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import threading
+import inspect
+import os
+import logging
+from ctypes import *
+from time import sleep
+
+DEBUG=0
+logger = logging.getLogger(__name__)
+
+class i2c:
+       libc = CDLL("libc.so.6")
+       I2C_SLAVE = 0x0703  # Use this slave address
+       __single = None
+       __lock = threading.Lock()
+       __parent_owner = None
+
+       def __init__(self, addr):
+               with i2c.__lock:
+                       count = 0
+                       while(i2c.__single):
+                               parent = inspect.stack()[1][3]
+                               count += 1
+                               sleep(0.001)
+                       if DEBUG:
+                               if count > 10:
+                                       parent_owner = "%s (%d), %s()" % (self.__parent_owner[1], self.__parent_owner[2], self.__parent_owner[3])
+                                       logger.warning("Error: (%s) I2C blocked %fs by %s!", parent, count*0.001, parent_owner)
+                               i2c.__parent_owner = inspect.stack()[1]
+                       i2c.__single = True
+               self.dev = i2c.libc.open("/dev/i2c-2", os.O_RDWR)
+               if self.dev < 0:
+                       raise IOError("open")
+               err = i2c.libc.ioctl(self.dev, i2c.I2C_SLAVE, addr>>1)
+               if err < 0:
+                       raise IOError("ioctl")
+
+       def write(self, s):
+               num_write = i2c.libc.write(self.dev, s, len(s))
+               if num_write != len(s):
+                       self.close()
+                       raise IOError("write: %d" % (num_write))
+       
+       def read(self, num):
+               buf = create_string_buffer(num)
+               num_read = i2c.libc.read(self.dev, buf, num)
+               if num_read != num:
+                       self.close()
+                       raise IOError("read: %d" % (num_read))
+               return buf.raw
+
+       def close(self):
+               if self.dev:
+                       i2c.libc.close(self.dev)
+                       self.dev = None
+                       #i2c.__parent_owner = None
+                       i2c.__single = None
+
+       def __del__(self):
+               self.close()
+
+if __name__ == "__main__":
+       import struct
+       import sys
+
+       dev = i2c(0x50)
+       s = struct.pack(">Bh", int(sys.argv[1]), int(sys.argv[2]))
+       dev.write(s)
+       dev.close()
diff --git a/scripts/move_base.py b/scripts/move_base.py
new file mode 100755 (executable)
index 0000000..1188e2c
--- /dev/null
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import rospy
+import struct
+from i2c import i2c
+from math import *
+from geometry_msgs.msg import Twist
+
+WHEEL_DIST = 0.248
+
+class MoveBase:
+       def __init__(self):
+               rospy.init_node('wild_thumper_move_base')
+               rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived)
+               self.set_speed(0, 0)
+               rospy.loginfo("Init done")
+               self.run()
+       
+       def run(self):
+               rate = rospy.Rate(10.0)
+               while not rospy.is_shutdown():
+                       rate.sleep()
+
+       def set_speed(self, left, right):
+               if left > 255: left=255
+               elif left < -255: left=-255
+               if right > 255: right=255
+               elif right < -255: right=-255
+
+               dev = i2c(0x56)
+               s = struct.pack(">Bhh", 0x1, left, right)
+               dev.write(s)
+               dev.close()
+
+       def cmdVelReceived(self, msg):
+               trans = msg.linear.x
+               rot = msg.angular.z # rad/s
+
+               right = rot*pi*WHEEL_DIST + trans
+               left = trans*2-right
+               self.set_speed(left, right)
+
+
+if __name__ == "__main__":
+       MoveBase()