Skip to content

[WIP] Add Wifi support (rooWifi adapter) #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Theosakamg wants to merge 1 commit into
base: indigo
Choose a base branch
from
Open

[WIP] Add Wifi support (rooWifi adapter) #27

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
181 changes: 156 additions & 25 deletions create_driver/src/create_driver/create_driver.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
# for consistency ROS Python coding guidelines.

from __future__ import with_statement
from abc import ABCMeta, abstractmethod

"""iRobot Roomba Serial control Interface (SCI) and Turtlebot Open Interface (OI).

Expand All @@ -48,6 +49,7 @@
import logging
import math
import serial
import socket
import struct
import time
import threading
Expand Down Expand Up @@ -202,14 +204,15 @@
class DriverError(Exception):
pass

class SerialCommandInterface(object):
class BaseCommandInterface(object):
__metaclass__ = ABCMeta

"""A higher-level wrapper around PySerial specifically designed for use with
iRobot's SCI.

"""
def __init__(self, tty, baudrate):
self.ser = serial.Serial(tty, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
logging.debug('Connect to Command interface.')
self.wake()
self.opcodes = {}

Expand All @@ -219,6 +222,58 @@ def __init__(self, tty, baudrate):

def wake(self):
"""wake up robot."""
logging.debug('wake up robot.')
print "==> wake up robot."

def add_opcodes(self, opcodes):
"""Add available opcodes to the SCI."""
logging.debug('Add available opcodes to the SCI.')
print "==> Add available opcodes to the SCI. API :" + ('Create', 'Roomba')[opcodes == ROOMBA_OPCODES]
self.opcodes.update(opcodes)

@abstractmethod
def send(self, bytes):
"""send a string of bytes to the robot."""
pass

@abstractmethod
def read(self, num_bytes):
"""Read a string of 'num_bytes' bytes from the robot."""
pass

@abstractmethod
def flush_input(self):
"""Flush input buffer, discarding all its contents."""
pass

def __getattr__(self, name):
"""Turtlebots methods for opcodes on the fly.

Each opcode method sends the opcode optionally followed by a string of
bytes.

"""
#TODO: kwc do static initialization instead
if name in self.opcodes:
def send_opcode(*bytes):
logging.debug('sending opcode %s.' % name)
self.send([self.opcodes[name]] + list(bytes))
return send_opcode
raise AttributeError

class SerialCommandInterface(BaseCommandInterface):

"""Serial Interface for Command Interfarce.

"""

def __init__(self, tty, baudrate):
logging.info('Connect by Serial %s at %d', tty, baudrate)
self.ser = serial.Serial(tty, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
super(SerialCommandInterface, self).__init__(tty, baudrate)

def wake(self):
super(SerialCommandInterface, self).wake()
self.ser.setRTS(0)
time.sleep(0.1)
self.ser.setRTS(1)
Expand All @@ -227,20 +282,14 @@ def wake(self):
self.ser.setRTS(0)
time.sleep(0.25)
self.ser.setRTS(1)
time.sleep(0.25)

def add_opcodes(self, opcodes):
"""Add available opcodes to the SCI."""
self.opcodes.update(opcodes)
time.sleep(0.25)

def send(self, bytes):
"""send a string of bytes to the robot."""
with self.lock:
self.ser.write(struct.pack('B' * len(bytes), *bytes))

#TODO: kwc the locking should be done at a higher level
def read(self, num_bytes):
"""Read a string of 'num_bytes' bytes from the robot."""
logging.debug('Attempting to read %d bytes from SCI port.' % num_bytes)
with self.lock:
data = self.ser.read(num_bytes)
Expand All @@ -252,25 +301,103 @@ def read(self, num_bytes):
return data

def flush_input(self):
"""Flush input buffer, discarding all its contents."""
logging.debug('Flushing serial input buffer.')
self.ser.flushInput()


def __getattr__(self, name):
"""Turtlebots methods for opcodes on the fly.
class WifiCommandInterface(BaseCommandInterface):

Each opcode method sends the opcode optionally followed by a string of
bytes.
"""Wifi Interface for Command Interfarce.

"""
#TODO: kwc do static initialization instead
if name in self.opcodes:
def send_opcode(*bytes):
logging.debug('sending opcode %s.' % name)
self.send([self.opcodes[name]] + list(bytes))
return send_opcode
raise AttributeError
"""

def __init__(self, ip, port):
logging.info('Connect by Wifi at %s:%d', ip, port)
self.soc = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.soc.settimeout(2)
self.soc.connect((ip , port)) # See more at: http://www.roowifi.com/sample-python-gtk/#sthash.nIjf68gl.dpuf
super(WifiCommandInterface, self).__init__(ip, port)

def wake(self):
super(WifiCommandInterface, self).wake()
time.sleep(0.1)

def send(self, bytes):
with self.lock:
data = struct.pack('B' * len(bytes), *bytes)
self.soc.sendall(data)
#print "==> Send a string of " + str(len(data)) + "/" + str(len(bytes)) + " byte(s) to the robot. " + str(bytes)

#TODO: kwc the locking should be done at a higher level
def read(self, num_bytes):
#logging.debug("Attempting to read " + str(num_bytes) + " bytes from SCI port.")
#print "==> Attempting to read " + str(num_bytes) + " bytes from SCI port."
buff = bytearray(num_bytes)
view = memoryview(buff)
toread = num_bytes
with self.lock:
while toread:
nbytes = self.soc.recv_into(view, toread/4, socket.MSG_WAITALL)
view = view[nbytes:]
toread -= nbytes
lengt = len(buff)
#print "=> reading " + str(lengt) + " bytes."
logging.debug('Read %d bytes from SCI port.' % lengt)
if not buff:
raise DriverError('Error reading from SCI port. No data.')
if lengt != (num_bytes):
raise DriverError('Error reading from SCI port. Wrong data length.')
return buff

def flush_input(self):
logging.debug('Flushing serial input buffer.')

class BtCommandInterface(BaseCommandInterface):

"""Bluetooth Interface for Command Interfarce.

"""

def __init__(self, ip, port):
logging.info('Connect by Wifi at %s:%d', ip, port)
#self.bt =
super(BtCommandInterface, self).__init__(ip, port)

def wake(self):
super(BtCommandInterface, self).wake()
time.sleep(0.1)

def send(self, bytes):
with self.lock:
data = struct.pack('B' * len(bytes), *bytes)
#self.bt.send(data)
#print "==> Send a string of " + str(len(data)) + "/" + str(len(bytes)) + " byte(s) to the robot. " + str(bytes)

#TODO: kwc the locking should be done at a higher level
def read(self, num_bytes):
#logging.debug("Attempting to read " + str(num_bytes) + " bytes from SCI port.")
#print "==> Attempting to read " + str(num_bytes) + " bytes from SCI port."
nbytes = 0
buff = bytearray(num_bytes)
view = memoryview(buff)
toread = num_bytes
with self.lock:
while toread:
#TODO map good implement !!!
#nbytes = self.soc.recv_into(view, toread/4, socket.MSG_WAITALL)
view = view[nbytes:]
toread -= nbytes
lengt = len(buff)
#print "=> reading " + str(lengt) + " bytes."
logging.debug('Read %d bytes from SCI port.' % lengt)
if not buff:
raise DriverError('Error reading from SCI port. No data.')
if lengt != (num_bytes):
raise DriverError('Error reading from SCI port. Wrong data length.')
return buff

def flush_input(self):
logging.debug('Flushing serial input buffer.')

class Roomba(object):

Expand All @@ -283,7 +410,8 @@ def __init__(self):

def start(self, tty='/dev/ttyUSB0', baudrate=57600):
self.tty = tty
self.sci = SerialCommandInterface(tty, baudrate)
#self.sci = SerialCommandInterface(tty, baudrate)
self.sci = WifiCommandInterface('192.168.0.30', 9001)
self.sci.add_opcodes(ROOMBA_OPCODES)

def change_baud_rate(self, baud_rate):
Expand All @@ -301,8 +429,11 @@ def change_baud_rate(self, baud_rate):
"""
if baud_rate not in BAUD_RATES:
raise DriverError('Invalid baud rate specified.')
self.sci.baud(baud_rate)
self.sci = SerialCommandInterface(self.tty, baud_rate)
if self.sci is SerialCommandInterface:
self.sci.baud(baud_rate)
#self.sci = SerialCommandInterface(self.tty, baud_rate)
#else
#self.sci = WifiCommandInterface('192.168.0.30', 9001)

def passive(self):
"""Put the robot in passive mode."""
Expand Down