jens/opencv
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

- added ev3

Browse Source 
git-svn-id: http://moon:8086/svn/software/trunk/projects/opencv@329 b431acfa-c32f-4a4a-93f1-934dc6c82436
This commit is contained in:
jens
2016-10-22 06:43:13 +00:00
parent 52747ba516
commit 7412e813bc
6 changed files with 3091 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ev3/__init__.py
Executable
+12
View File
@@ -0,0 +1,12 @@
import sys
import os
thisdir = os.path.dirname(os.path.abspath(__file__))
print(thisdir)
sys.path.append(thisdir)
import ev3
import message
import direct_command
import system_command
import async
ev3/async.py
Executable
+91
View File
@@ -0,0 +1,91 @@
"""A simple thread subclass for making ev3 function calls asynchronous.
EXAMPLE USAGE:
import time
from ev3 import *
finished = False
def keep_alive_finished(result):
global finished
print 'The keep_alive() function returned: ', result
finished = True
if ("__main__" == __name__):
try:
async_thread = async.AsyncThread()
with ev3.EV3() as brick:
async_thread.put(brick.keep_alive, keep_alive_finished)
while (not finished):
print 'Waiting...'
time.sleep(0.1)
except ev3.EV3Error as ex:
print 'An error occurred: ', ex
async_thread.stop()
"""
import threading
import queue
class AsyncThread(threading.Thread):
"""A simple thread subclass maintains a queue of functions to call."""
_STOP_QUEUE_ITEM = 'STOP'
def __init__(self):
"""Creates and starts a new thread."""
super(AsyncThread, self).__init__()
self._daemon = True
self._queue = Queue.Queue()
self.start()
def run(self):
"""This function is called automatically by the Thread class."""
try:
while(True):
item = self._queue.get(block=True)
if (self._STOP_QUEUE_ITEM == item):
break
ev3_func, cb, args, kwargs = item
cb(ev3_func(*args, **kwargs))
except KeyboardInterrupt:
pass
def stop(self):
"""Instructs the thread to exit after the current function is
finished.
"""
with self._queue.mutex:
self._queue.queue.clear()
self._queue.put(self._STOP_QUEUE_ITEM)
def put(self, ev3_func, cb, *args, **kwargs):
"""Adds a new function to the queue. The cb (callback) parameter should
be a function that accepts the result as its only parameter.
"""
self._queue.put((ev3_func, cb, args, kwargs))
ev3/direct_command.py
Executable
+2213
View File
File diff suppressed because it is too large Load Diff
ev3/ev3.py
Executable
+188
View File
@@ -0,0 +1,188 @@
"""A wrapper for using Python to interact with a Lego Mindstorms EV3.
Prerequisites:
Make sure your sdp includes an SP (serial port):
% sdptool browse local
...
Service Name: Serial Port
Service Description: COM Port
If not:
% sdptool add SP
Then:
% hcitool scan
...
XX:XX:XX:XX:XX:XX EV3
% sudo rfcomm bind /dev/rfcomm0 XX:XX:XX:XX:XX:XX
Now /dev/rfcomm0 can be opened and closed like a normal serial port.
The opposite action is:
% sudo rfcomm release /dev/rfcomm0
EXAMPLE USAGE:
from ev3 import *
with ev3.EV3() as brick:
# Create DirectCommand objects and add commands to them.
cmd = direct_command.DirectCommand()
cmd.add_ui_draw_update()
cmd.send(brick)
# Call single DirectCommand functions without creating DirectCommand
# objects each time.
brick.output_stop(direct_command.OutputPort.PORT_C,
direct_command.StopType.BRAKE)
# Call single system_command functions.
brick.write_mailbox('foo', (0,1,2,3,4,5,6,7,8,9,0))
"""
import serial
import message
import system_command
import direct_command
class KnownPaths(object):
"""These are the default directories on the brick. All paths are
relative to 'lms2012/sys' by default.
"""
DEFAULT_PATH = "." # lms2012/sys
PROJECTS_PATH = "../prjs" # lms2012/prjs
APPS_PATH = "../apps" # lms2012/apps
TOOLS_PATH = "../tools" # lms2012/tools
SOURCE_PATH = "../source" # lms2012/source
class EV3Error(Exception):
"""Subclass for reporting errrors."""
pass
class EV3(object):
""""""
DEFAULT_RFCOMM_PORT = '/dev/rfcomm0'
RFCOMM_BAUDRATE = 115200
def __init__(self, port_str=DEFAULT_RFCOMM_PORT):
"""Creates a new object but doesn't open the port."""
self._port_str = port_str
self._port = None
def open(self):
"""Opens the object's serial port."""
if (self._port is None):
self._port = serial.Serial(port=self._port_str,
baudrate=self.RFCOMM_BAUDRATE,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=None,
xonxoff=False,
rtscts=False,
writeTimeout=None,
dsrdtr=False,
interCharTimeout=None)
def close(self):
"""Closes the object's serial port."""
if (self._port is not None):
self._port.close()
self._port = None
def send_message(self, msg, message_counter=0x1234):
"""Allows for sending raw messages to the EV3. The msg parameter should
be an array of byte values. The msg parameter should not include the
length/message_counter header. Raises an EV3Error if the specified
message is a type that expects a reply.
"""
try:
message.send_message_no_reply(self._port, msg, message_counter)
except message.MessageError as ex:
raise EV3Error(ex.message)
def send_message_for_reply(self, msg, message_counter=0x1234):
"""Allows for sending raw messages to the EV3. The msg parameter should
be an array of byte values. The msg parameter should not include the
length/message_counter header. Raises an EV3Error if the specified
message is a type that doesn't expect a reply.
"""
try:
return message.send_message_for_reply(self._port,
msg,
message_counter)
except message.MessageError as ex:
raise EV3Error(ex.message)
def __dir__(self):
"""Add in functions from the system_command module as well as methods
from the DirectCommand class because they can be called directly on an
EV3 object.
"""
result = dir(type(self))
result += list(self.__dict__)
result += [s for s in list(system_command.__dict__)
if not s.startswith('_')]
result += [s[4:] for s in list(direct_command.DirectCommand.__dict__)
if s.startswith('add_')]
return sorted(set(result))
def __getattr__(self, name):
"""A little bit of magic is used in order to make it easier to work with
EV3 objects.
"""
if (hasattr(system_command, name)):
# This allows functions from the system_command module to be called
# from an EV3 object i.e. ev3.list_files(KnownPaths.PROJECTS_PATH).
def execute_sc(*args):
"""This is just a wrapper around an individual function from the
system_command module. See the system_command module for more
information.
"""
getattr(system_command, name)(self, *args)
return execute_sc
# This allows single functions from the DirectCommand class to be called
# from an EV3 object i.e. ev3.ui_draw_update().
dc_name = ('add_' + name)
if (hasattr(direct_command.DirectCommand, dc_name)):
def execute_dc(*args):
"""This is just a wrapper around an individual DirectCommand
method. See the DirectCommand class for more information.
"""
dc = direct_command.DirectCommand()
getattr(dc, dc_name)(*args)
return dc.send(self)
return execute_dc
return Object.__getattr__(self, name)
def __enter__(self):
self.open()
return self
def __exit__(self, type, value, traceback):
self.close()
ev3/message.py
Executable
+166
View File
@@ -0,0 +1,166 @@
"""Handles the messaging with EV3 and contains several functions for dealing
with message variable types.
"""
import struct
import system_command
import direct_command
class MessageError(Exception):
"""Subclass for reporting errors."""
pass
def send_message_for_reply(port, msg, message_counter=0x1234):
"""Sends the message and waits for a reply. The msg is expected to be a
sequence of bytes and it should not contain the length/message_counter
header. Returns an sequence of bytes without the length/message_counter
header.
"""
if (not msg_expects_reply(msg)):
raise MessageError('The message is not a type that expects a reply.')
# Message length includes the two message_counter bytes.
msg_len = (2 + len(msg))
msg_counter_lsb = (message_counter & 0xFF)
msg_counter_msb = ((message_counter >> 8) & 0xFF)
buf = [(msg_len & 0xFF),
((msg_len >> 8) & 0xFF),
msg_counter_lsb,
msg_counter_msb]
buf += msg
_write_bytes(port, buf)
expected_len = _read_bytes(port, 2)
expected_len = (expected_len[0] | (expected_len[1] << 8))
reply = _read_bytes(port, expected_len)
if (reply[0] != msg_counter_lsb or reply[1] != msg_counter_msb):
raise MessageError('Reply message counter does not match.')
return reply[2:]
def send_message_no_reply(port, msg, message_counter=0x1234):
"""Sends the message without waiting for a reply."""
if (msg_expects_reply(msg)):
raise MessageError('The message is a type that expects a reply.')
# Message length includes the two message_counter bytes.
msg_len = (2 + len(msg))
msg_counter_lsb = (message_counter & 0xFF)
msg_counter_msb = ((message_counter >> 8) & 0xFF)
buf = [(msg_len & 0xFF),
((msg_len >> 8) & 0xFF),
msg_counter_lsb,
msg_counter_msb]
buf += msg
_write_bytes(port, buf)
def msg_expects_reply(msg):
"""Returns True if the given message is a type that expects a reply. The
given message should not include the length/message_counter header.
"""
if (system_command.CommandType.SYSTEM_COMMAND_REPLY == msg[0]):
return True
if (direct_command.CommandType.DIRECT_COMMAND_REPLY == msg[0]):
return True
return False
def parse_u16(byte_seq, index):
"""Parses a u32 value at the given index from the byte_seq."""
return (byte_seq[index] | (byte_seq[index + 1] << 8))
def parse_u32(byte_seq, index):
"""Parses a u32 value at the given index from the byte_seq."""
return (byte_seq[index] |
(byte_seq[index + 1] << 8) |
(byte_seq[index + 2] << 16) |
(byte_seq[index + 3] << 24))
def parse_str(byte_seq, index, length=None):
"""Parses a string of length chars."""
if (length is None):
return ''.join([chr(c) for c in byte_seq[index:]])
else:
return ''.join([chr(c) for c in byte_seq[index:(index + length)]])
def parse_null_terminated_str(byte_seq, index, length):
"""Parses a null-terminated string of up to length chars."""
result = []
for i in range(index, (index + length)):
if (0x00 != byte_seq[i]):
result.append(chr(byte_seq[i]))
else:
break
return ''.join(result)
def parse_float(byte_seq, index):
"""Parses a 32bit floating point number."""
str_value = ''.join([chr(c) for c in byte_seq[index:(index + 4)]])
return struct.unpack('<f', str_value)[0]
def append_float(byte_list, value):
"""Appends a 32bit floating point number."""
value_str = struct.pack('<f', value).ljust(4, '\0')
map(byte_list.append, [ord(c) for c in value_str])
def append_u8(byte_list, value):
"""Appends the given value to the list."""
byte_list.append(value & 0xFF)
def append_u16(byte_list, value):
"""Appends the given value to the list in little-endian order."""
byte_list.append(value & 0xFF)
byte_list.append((value >> 8) & 0xFF)
def append_u32(byte_list, value):
"""Appends the given value to the list in little-endian order."""
byte_list.append(value & 0xFF)
byte_list.append((value >> 8) & 0xFF)
byte_list.append((value >> 16) & 0xFF)
byte_list.append((value >> 24) & 0xFF)
def append_str(byte_list, str_value):
"""Appends a null-terminated string."""
if ('\0' != str_value[-1]):
str_value += '\0'
for c in str_value:
byte_list.append(ord(c))
def _read_bytes(port, num_bytes):
return [ord(i) for i in port.read(num_bytes)]
def _write_bytes(port, byte_seq):
port.write(byte_seq)
ev3/system_command.py
Executable
+421
View File
@@ -0,0 +1,421 @@
"""System commands are defined as commands that aren't executed in byte code
(no VM intervention).
All multi-byte words are little endian.
System Command Bytes:
------------------------------
Byte 0 - 1: Command size
Byte 2 - 3: Message counter
Byte 4: CommandType
Byte 5: Command
Byte 6 - n: payload
System Command response Bytes:
------------------------------
Byte 0 - 1: Reply size
Byte 2 - 3: Message counter
Byte 4: ReplyType
Byte 5: original Command
Byte 6: ReturnCode
Byte 7 - N: payload
"""
import itertools
import message
MAX_REPLY_BYTES = 1014 # According to c_com.h comments.
MAX_TX_BYTES = 1016
class SystemCommandError(Exception):
"""Subclass for reporting errors."""
pass
class CommandType(object):
"""Every System Command must be one of these two types."""
SYSTEM_COMMAND_REPLY = 0x01
SYSTEM_COMMAND_NO_REPLY = 0x81
class ReplyType(object):
"""Every reply to a System Command must be one of these two types."""
SYSTEM_REPLY = 0x03
SYSTEM_REPLY_ERROR = 0x05
class Command(object):
"""Enumerated System Commands."""
BEGIN_DOWNLOAD = 0x92 # Begin file down load
CONTINUE_DOWNLOAD = 0x93 # Continue file down load
BEGIN_UPLOAD = 0x94 # Begin file upload
CONTINUE_UPLOAD = 0x95 # Continue file upload
BEGIN_GETFILE = 0x96 # Begin get bytes from a file (while writing to the file)
CONTINUE_GETFILE = 0x97 # Continue get byte from a file (while writing to the file)
CLOSE_FILEHANDLE = 0x98 # Close file handle
LIST_FILES = 0x99 # List files
CONTINUE_LIST_FILES = 0x9A # Continue list files
CREATE_DIR = 0x9B # Create directory
DELETE_FILE = 0x9C # Delete
LIST_OPEN_HANDLES = 0x9D # List handles
WRITEMAILBOX = 0x9E # Write to mailbox
BLUETOOTHPIN = 0x9F # Transfer trusted pin code to brick
ENTERFWUPDATE = 0xA0 # Restart the brick in Firmware update mode
SETBUNDLEID = 0xA1 # Set Bundle ID for mode 2
SETBUNDLESEEDID = 0xA2 # Set Bundle Seed ID for mode 2
class ReturnCode(object):
"""Enumerated System Command return codes."""
SUCCESS = 0x00
UNKNOWN_HANDLE = 0x01
HANDLE_NOT_READY = 0x02
CORRUPT_FILE = 0x03
NO_HANDLES_AVAILABLE = 0x04
NO_PERMISSION = 0x05
ILLEGAL_PATH = 0x06
FILE_EXITS = 0x07
END_OF_FILE = 0x08
SIZE_ERROR = 0x09
UNKNOWN_ERROR = 0x0A
ILLEGAL_FILENAME = 0x0B
ILLEGAL_CONNECTION = 0x0C
def write_mailbox(ev3_obj, mailbox_name_str, msg):
"""Writes a sequence of bytes to the mailbox with the given name."""
if ('\0' != mailbox_name_str[-1]):
mailbox_name_str += '\0'
if isinstance(msg, str):
byte_seq = (msg + '\0').encode('ascii')
else:
byte_seq = msg
cmd = []
cmd += CommandType.SYSTEM_COMMAND_NO_REPLY.to_bytes(1, byteorder='little')
cmd += Command.WRITEMAILBOX.to_bytes(1, byteorder='little')
cmd += len(mailbox_name_str).to_bytes(1, byteorder='little')
cmd += mailbox_name_str.encode('ascii')
cmd += len(byte_seq).to_bytes(2, byteorder='little')
cmd += byte_seq
ev3_obj.send_message(cmd)
def list_files(ev3_obj, path_str):
"""Returns a tuple in the form (DIRS, FILES). DIRS is a tuple of directory
names (i.e. 'foo/'). FILES is a tuple of file information tuples in the form
(MD5_SUM, FILE_LENGTH, FILE_NAME).
"""
dirs = []
files = []
list_str = ''
continue_list_str = ''
if (not isinstance(path_str, str)):
raise ValueError('The path_str param must be of type str.')
list_str, handle, needs_continue = _list_files(ev3_obj, path_str)
if (needs_continue):
continue_list_str = _continue_list_files(ev3_obj, handle)
result = (list_str + continue_list_str)
for line in result.strip().split('\n'):
if (line.endswith('/')):
# Directories have the format: '[DIR_NAME]/\n'
dirs.append(line)
elif (line):
# Files have the format: '[MD5] [HEX_SIZE] [FILE_NAME]\n'
fields = line.split()
files.append((fields[0], int(fields[1], 16), ' '.join(fields[2:])))
return (dirs, files)
def upload_file(ev3_obj, path_str, save_path_str=None):
"""Uploads the file from the given path on the brick to the PC. If save_path_str
is not None then the file will be written to disk. Otherwise, the file data
will be returned as a tuple of bytes.
"""
if (not isinstance(path_str, str)):
raise ValueError('The path_str param must be of type str.')
result, handle, needs_continue = _upload_file(ev3_obj, path_str)
if (needs_continue):
result += _continue_upload_file(ev3_obj, handle)
if (save_path_str is not None):
with open(save_path_str, 'w') as out_file:
out_file.write(message.parse_str(result, 0, len(result)))
else:
return tuple(result)
def download_file_from_path(ev3_obj, save_path_str, file_path_str):
"""Downloads the file from file_path_str on the PC to save_path_str on the brick.
NOTE: This function creates intermediary directories automatically.
"""
if (not isinstance(file_path_str, str)):
raise ValueError('The data_path_str param must be of type str.')
with open(file_path_str, 'r') as read_file:
return download_file(ev3_obj, read_file.read(), save_path_str)
def download_file(ev3_obj, save_path_str, file_data):
"""Downloads the file_data to save_path_str on the brick.
NOTE: This function creates intermediary directories automatically.
"""
if (not isinstance(save_path_str, str)):
raise ValueError('The save_path_str param must be of type str.')
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.BEGIN_DOWNLOAD)
message.append_u32(cmd, len(file_data))
message.append_str(cmd, save_path_str)
reply = ev3_obj.send_message_for_reply(cmd)
print ('reply: ' + str(reply))
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.BEGIN_DOWNLOAD):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
handle = reply[3]
_continue_download_file(ev3_obj, handle, file_data)
def create_dir(ev3_obj, path_str):
"""Creates the directory at the given path_str."""
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_NO_REPLY)
cmd.append(Command.CREATE_DIR)
message.append_str(cmd, path_str)
ev3_obj.send_message(cmd)
def delete_path(ev3_obj, path_str):
"""Deletes the file or directory specified by the given path_str.
NOTE: Directories must be empty before they can be deleted.
"""
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_NO_REPLY)
cmd.append(Command.DELETE_FILE)
message.append_str(cmd, path_str)
ev3_obj.send_message(cmd)
def delete_directory(ev3_obj, dir_path_str):
"""Convenience function for deleting directories that may or may not be
empty.
"""
if (not dir_path_str.endswith('/')):
dir_path_str += '/'
directories, files = list_files(ev3_obj, dir_path_str)
for f in files:
md5, length, file_name = f
delete_path_str(ev3_obj, (dir_path_str + file_name))
for d in directories:
# Ignore './' and '../'.
if (not d.startswith('.')):
delete_directory(ev3_obj, (dir_path_str + d))
delete_path_str(ev3_obj, dir_path_str)
def _list_files(ev3_obj, path_str):
handle = None
needs_continue = False
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.LIST_FILES)
message.append_u16(cmd, MAX_REPLY_BYTES)
message.append_str(cmd, path_str)
reply = ev3_obj.send_message_for_reply(cmd)
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.LIST_FILES):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
list_size = message.parse_u32(reply, 3)
handle = reply[7]
result = message.parse_str(reply, 8)
return (result, handle, (reply[2] != ReturnCode.END_OF_FILE))
def _continue_list_files(ev3_obj, handle):
result = []
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.CONTINUE_LIST_FILES)
cmd.append(handle)
message.append_u16(cmd, MAX_REPLY_BYTES)
while (True):
reply = ev3_obj.send_message_for_reply(cmd)
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.CONTINUE_LIST_FILES):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
handle = reply[3]
result.append(message.parse_str(reply, 4))
if (reply[2] == ReturnCode.END_OF_FILE):
break
return ''.join(result)
def _upload_file(ev3_obj, path_str):
handle = None
needs_continue = False
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.BEGIN_UPLOAD)
message.append_u16(cmd, MAX_REPLY_BYTES)
message.append_str(cmd, path_str)
reply = ev3_obj.send_message_for_reply(cmd)
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.BEGIN_UPLOAD):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
data_size = message.parse_u32(reply, 3)
handle = reply[7]
result = reply[8:]
return (result, handle, (reply[2] != ReturnCode.END_OF_FILE))
def _continue_upload_file(ev3_obj, handle):
result = []
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.CONTINUE_UPLOAD)
cmd.append(handle)
message.append_u16(cmd, MAX_REPLY_BYTES)
while (True):
reply = ev3_obj.send_message_for_reply(cmd)
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.CONTINUE_UPLOAD):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
handle = reply[3]
result.append(reply[4:])
if (reply[2] == ReturnCode.END_OF_FILE):
break
return itertools.chain.from_iterable(result)
def _continue_download_file(ev3_obj, handle, data):
offset = 0
data_len = len(data)
cmd = []
cmd.append(CommandType.SYSTEM_COMMAND_REPLY)
cmd.append(Command.CONTINUE_DOWNLOAD)
cmd.append(handle)
if (MAX_TX_BYTES >= data_len):
offset = data_len
else:
offset = MAX_TX_BYTES
map(cmd.append, data[:offset])
while (True):
reply = ev3_obj.send_message_for_reply(cmd)
if (reply[0] == ReplyType.SYSTEM_REPLY_ERROR):
raise SystemCommandError('A command failed.')
if (reply[1] != Command.CONTINUE_DOWNLOAD):
raise SystemCommandError('Sync error detected.')
if (reply[2] == ReturnCode.UNKNOWN_ERROR):
raise SystemCommandError('An error occurred.')
if (data_len == offset):
break
del (cmd[3:])
if (MAX_TX_BYTES >= (data_len - offset)):
map(cmd.append, data[offset:])
offset = data_len
else:
new_offset = (offset + MAX_TX_BYTES)
map(cmd.append, data[offset:new_offset])
offset = new_offset