From 3d875e0b7d29c59be6a65b13cd4e2b0d157b6744 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sat, 4 Apr 2015 20:20:29 +0200
Subject: [PATCH] added teleop, move_base for cmd_vel
---
launch/teleop.launch | 13 +++
scripts/bootloader.py | 179 ++++++++++++++++++++++++++++++++++++++++++
scripts/i2c.py | 72 +++++++++++++++++
scripts/move_base.py | 46 +++++++++++
4 files changed, 310 insertions(+)
create mode 100644 launch/teleop.launch
create mode 100755 scripts/bootloader.py
create mode 100755 scripts/i2c.py
create mode 100755 scripts/move_base.py
diff --git a/launch/teleop.launch b/launch/teleop.launch
new file mode 100644
index 0000000..60f4adf
--- /dev/null
+++ b/launch/teleop.launch
@@ -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
index 0000000..2abcb93
--- /dev/null
+++ b/scripts/bootloader.py
@@ -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
index 0000000..2e9a109
--- /dev/null
+++ b/scripts/i2c.py
@@ -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
index 0000000..1188e2c
--- /dev/null
+++ b/scripts/move_base.py
@@ -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()
--
2.39.5