class BtServer(object):
def __init__(self):
super(BtServer, self).__init__()
self.socket = BluetoothSocket(RFCOMM)
self.client = {}
def start(self):
# empty host address means this machine
self.socket.bind(("", PORT_ANY))
self.socket.listen(config['backlog'])
self.port = self.socket.getsockname()[1]
uuid = config['uuid']
advertise_service(
self.socket,
"Rewave Server",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE]
)
print('Waiting for connection on RFCOMM channel %d' % self.port)
self.client['socket'], self.client['info'] = self.socket.accept()
print("Accepted connection from ", self.client['info'])
def kill(self):
self.socket.close()
def close_connection(self):
self.client['socket'].close()
class SimulatedKeyboardDevice:
def __init__(self):
os.system("hciconfig hci0 up")
os.system("hciconfig hci0 class 0x002540")
os.system("hciconfig hci0 name " + DEVICE_NAME)
os.system("hciconfig hci0 piscan")
opts = {
"ServiceRecord": read_service_record(),
"Role": "server",
"RequireAuthentication": False,
"RequireAuthorization": False,
}
bus = dbus.SystemBus()
manager = dbus.Interface(bus.get_object("org.bluez", "/org/bluez"), "org.bluez.ProfileManager1")
profile = KeyboardProfile(bus, PROFILE_DBUS_PATH)
manager.RegisterProfile(PROFILE_DBUS_PATH, UUID, opts)
self.control_socket = BluetoothSocket(L2CAP)
self.interrupt_socket = BluetoothSocket(L2CAP)
self.control_socket.setblocking(0)
self.interrupt_socket.setblocking(0)
self.control_socket.bind(("", CONTROL_PORT))
self.interrupt_socket.bind(("", INTERRUPT_PORT))
def listen(self):
print "Waiting for a connection"
self.control_socket.listen(1)
self.interrupt_socket.listen(1)
self.control_channel = None
self.interrupt_channel = None
gobject.io_add_watch(
self.control_socket.fileno(), gobject.IO_IN, self.accept_control)
gobject.io_add_watch(
self.interrupt_socket.fileno(), gobject.IO_IN,
self.accept_interrupt)
def accept_control(self, source, cond):
self.control_channel, cinfo = self.control_socket.accept()
print "Got a connection on the control channel from " + cinfo[0]
return True
def accept_interrupt(self, source, cond):
self.interrupt_channel, cinfo = self.interrupt_socket.accept()
print "Got a connection on the interrupt channel from " + cinfo[0]
return True
def send(self, message):
if self.interrupt_channel is not None:
self.interrupt_channel.send(message)
def main():
# Setup logging
setup_logging()
# We need to wait until Bluetooth init is done
time.sleep(10)
# Make device visible
os.system("hciconfig hci0 piscan")
# Create a new server socket using RFCOMM protocol
server_sock = BluetoothSocket(RFCOMM)
# Bind to any port
server_sock.bind(("", PORT_ANY))
# Start listening
server_sock.listen(1)
# Get the port the server socket is listening
port = server_sock.getsockname()[1]
# The service UUID to advertise
uuid = "7be1fcb3-5776-42fb-91fd-2ee7b5bbb86d"
# Start advertising the service
advertise_service(server_sock,
"RaspiBtSrv",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE])
# These are the operations the service supports
# Feel free to add more
operations = ["ping", "example"]
# Main Bluetooth server loop
while True:
print("Waiting for connection on RFCOMM channel %d" % port)
client_sock = None
try:
# This will block until we get a new connection
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)
# Read the data sent by the client
data = client_sock.recv(1024)
if len(data) == 0:
break
print("Received [%s]" % data)
# Handle the request
if data == "getop":
response = "op:%s" % ",".join(operations)
elif data == "ping":
response = "msg:Pong"
elif data == "example":
response = "msg:This is an example"
# Insert more here
else:
response = "msg:Not supported"
client_sock.send(response)
print("Sent back [%s]" % response)
except IOError:
pass
except KeyboardInterrupt:
if client_sock is not None:
client_sock.close()
server_sock.close()
print("Server going down")
break
def task(self):
# Waiting until Bluetooth init is done
time.sleep(self.wakeup_time)
# Make device visible
self.scm.set_bluetooth_visible()
# Create a new server socket using RFCOMM protocol
server_sock = BluetoothSocket(RFCOMM)
# Bind to any port
server_sock.bind(("", PORT_ANY))
# Start listening
server_sock.listen(self.max_simultaneous_connections)
# Get the port the server socket is listening
port = server_sock.getsockname()[1]
# Main bluetooth server loop
while True:
logger.info(
'Waiting for bluetooth connection on channel {}'.format(port))
client_sock = None
try:
# Block until we get a new connection
client_sock, client_info = server_sock.accept()
logger.info('Accepted connection from {}'.format(client_info))
# Read the data sent by the client
command = client_sock.recv(1024)
if len(command) == 0:
continue
logger.info('Received [{}]'.format(command))
# Handle the request
response = self.handle_request(command)
client_sock.send(response)
logger.info('Sent back [{}]'.format(response))
except IOError:
pass
except KeyboardInterrupt:
if client_sock is not None:
client_sock.close()
server_sock.close()
logger.info('Bluetooth server going down')
break
class BtServer(object):
def __init__(self, socket=None):
super(BtServer, self).__init__()
self.socket = socket
def start(self):
print('start called')
if not self.socket:
print('non socket, creating one')
self.socket = BluetoothSocket(RFCOMM)
# empty host address means this machine
self.socket.bind(("", PORT_ANY))
self.socket.listen(config['backlog'])
advertise_service(
self.socket,
"Rewave Server",
service_id=config['uuid'],
service_classes=[config['uuid'], SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE]
)
self.port = self.socket.getsockname()[1] # need port for ref.
self.accept()
def stop(self):
print("server stop called")
self.socket.close()
try:
self.client_socket.close()
except AttributeError as e:
# in case if no client is connected
pass
def recv(self, buffer=2048):
print('recv called')
try:
return self.client_socket.recv(buffer).decode(encoding='UTF-8')
except UnicodeDecodeError as e:
# return raw if unable to unicode decode
return self.client_socket.recv(buffer)
def send(self, data):
self.client_socket.send(data)
def close_connection(self):
print("close_connection called")
self.client_socket.close()
def accept(self):
# blocking call
print("accept Called. Waiting for connection on RFCOMM channel", self.port)
self.client_socket = self.socket.accept()[0] # returns socket, name
def run_bt_server(account):
"""Adapted from: https://github.com/EnableTech/raspberry-bluetooth-demo"""
import pdb
pdb.set_trace()
server_sock = BluetoothSocket(RFCOMM)
server_sock.bind(("", PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
print ("listening on port %d" % port)
uuid = "1e0ca4ea-299d-4335-93eb-27fcfe7fa848"
advertise_service(
server_sock,
"Mock Detector",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE],
# protocols=[OBEX_UUID],
)
print("Waiting for connection on RFCOMM channel %d" % port)
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)
try:
while True:
data = client_sock.recv(1024)
if len(data) == 0:
break
print("received [%s]" % data)
if data == b'ETH_ADDRESS\r\n':
client_sock.send(
"ETH_ADDRESS::0x{}\r\nEND\r\n".format(account.address())
)
elif data.startswith('SIGNED_MESSAGE::'):
start = len('SIGNED_MESSAGE::')
user_address = data[start:start+42]
message = account.create_signed_message(
user_address,
int(time.time())
)
client_sock.send(message)
except IOError:
pass
print("disconnected")
client_sock.close()
server_sock.close()
print("all done")
class BlueComm:
def __init__(self):
self.stop = False
self.exit = False
def start(self, callback):
self.stop = False
self.server_sock = BluetoothSocket(RFCOMM)
self.server_sock.bind(("", PORT_ANY))
self.server_sock.listen(1)
port = self.server_sock.getsockname()[1]
uuid = "fa87c0d0-afac-11de-8a39-0800200c9a66"
advertise_service(self.server_sock,
"blueCommServer",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE]
# protocols = [ OBEX_UUID ]
)
print("Waiting for connection on RFCOMM channel %d" % port)
self.client_sock, self.client_info = self.server_sock.accept()
print("Accepted connection from ", self.client_info)
try:
while not self.stop:
data = self.client_sock.recv(1024)
if len(data) == 0:
break
callback(self, data)
except IOError:
pass
print("disconnected")
self.disconnect()
def disconnect(self):
self.client_sock.close()
self.server_sock.close()
class RFCOMMServer:
"""Object that simplifies setting up an RFCOMM socket server.
This is based on the ``socketserver.SocketServer`` class in the Python
standard library.
"""
request_queue_size = 1
def __init__(self, server_address, RequestHandlerClass):
self.server_address = server_address
self.RequestHandlerClass = RequestHandlerClass
self.socket = BluetoothSocket(RFCOMM)
try:
self.socket.bind((server_address[0], server_address[1]))
# self.server_address = self.socket.getsockname()
self.socket.listen(self.request_queue_size)
except Exception:
self.server_close()
raise
def __enter__(self):
return self
def __exit__(self, type, value, traceback):
self.server_close()
def handle_request(self):
try:
request, addr_data = self.socket.accept()
except OSError:
return
try:
self.process_request(request, addr_data)
except Exception:
request.close()
raise
def process_request(self, request, client_address):
self.finish_request(request, client_address)
request.close()
def finish_request(self, request, client_address):
self.RequestHandlerClass(request, client_address, self)
def server_close(self):
self.socket.close()
class PantojoBluetoothReceiver:
PANTOJO_BLUETOOTH_PORT = 3
MAX_PACKET_SIZE = 34
def __init__(self):
self.server_socket = BluetoothSocket(RFCOMM)
self.server_socket.bind(("", self.PANTOJO_BLUETOOTH_PORT))
def open(self):
self.server_socket.listen(1)
self.client_socket, self.client_info = self.server_socket.accept()
def recv(self):
return self.client_socket.recv(self.MAX_PACKET_SIZE).strip()
def close(self):
self.client_socket.close()
self.server_socket.close()
def accept(self, check_pin=True):
self.env.send("pairable on")
self.env.send("discoverable on")
sock = BluetoothSocket(RFCOMM)
port = get_port()
sock.bind(("", port))
sock.listen(1)
Logger().write("[!] Waiting for a connection", tag="BTCTL")
while True:
client, addr = sock.accept()
mac = addr[0]
try:
msg = client.recv(8).decode().strip()
except Exception as ex:
client.close()
Logger().write(ex, tag="EXCEPT")
continue
if check_pin is True and msg != self.pin:
Logger().write("[!] Authentication failed", tag="BTCTL")
client.close()
continue
break
Logger().write("[!] Device %s has connected" % mac, tag="BTCTL")
try:
client.send(b"OK\n")
except Exception as ex:
Logger().write("[!] Device %s has disconnected" % mac, tag="BTCTL")
Logger().write(ex, tag="EXCEPT")
client.close()
return self.accept(check_pin)
self.env.send("discoverable off")
self.env.send("pairable off")
return client
class Server(object):
def __init__(self, loop):
self.loop = loop
self._serv_sock = BluetoothSocket(RFCOMM)
self._serv_sock.setblocking(0)
# self._serv_sock.setsockopt(SOL_SOCKET, SO_REUSEADDR, 1)
self._serv_sock.bind(("", PORT_ANY))
self._serv_sock.listen(5)
advertise_service(
self._serv_sock,
"Rewave Server",
service_id='a1a738e0-c3b3-11e3-9c1a-0800200c9a66',
service_classes=['a1a738e0-c3b3-11e3-9c1a-0800200c9a66', SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE]
)
self._peers = []
# Task(self._server())
self._server()
def remove(self, peer):
self._peers.remove(peer)
self.broadcast('Peer %s quit!\n' % (peer.name,))
def broadcast(self, message):
print(message)
for peer in self._peers:
peer.send(message)
def _server(self):
while True:
peer_sock, peer_name = yield from self._serv_sock.accept()
peer = Peer(self, peer_sock, peer_name)
self._peers.append(peer)
self.broadcast('Peer %s connected!\n' % (peer.name,))
def myThread(self,run_event):
server_sock=BluetoothSocket(RFCOMM)
#TODO:allow to change the port
server_sock.bind(("", PORT))
server_sock.listen(1)
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)
success=""
#success=struct.pack('8B',0xa0,0x00,0x08,0xcb,0x00,0x00,0x00,0x01) #success
success=struct.pack('3B',0xa0,0x00,0x03) #success
data = client_sock.recv(1024)
client_sock.send(success)
while run_event.is_set():
print('Waiting')
data = client_sock.recv(1024)
if len(data) == 0:
break
client_sock.send(success)
self.decodeMessage(data)
return
class BluetoothConnection(object):
def __init__(self):
self.client_socket = None
self.server_socket = None
self.connected = False
def createServer(self, port = 3):
self.server_socket=BluetoothSocket( RFCOMM )
self.server_socket.bind(("", port))
self.server_socket.listen(1)
print "Trying to connect.."
self.client_socket, _ = self.server_socket.accept()
print "Connected!"
self.connected = True
def createClient(self, address, port = 3):
if address == "first":
address = self.searchDevices()[0]
self.client_socket=BluetoothSocket( RFCOMM )
print "Trying to connect.."
self.client_socket.connect((address, port))
print "Connected!"
self.connected = True
def searchDevices(self):
return discover_devices()
def receiveData(self, bufsize = 1024):
if self.connected:
return self.client_socket.recv(bufsize)
else:
print "Not yet connected!"
def sendFile(self, filename):
f = open(filename, 'r')
data = f.readlines()
for line in data:
self.sendData(line)
f.close()
def receiveFile(self, filename, bufsize = 4096):
data = self.receiveData(bufsize)
f = open(filename, 'w')
f.writelines(data)
f.flush()
f.close()
def sendData(self, data):
if self.connected:
self.client_socket.send(data)
def closeConnection(self):
if self.server_socket is not None:
self.server_socket.close()
self.client_socket.close()
self.connected = False
def main():
"""
Main method of the application.
Must be run with superuser permissions.
Params: <user> <network_interface> <hotspot_ssid> <hotspot_password>
Creates the Bluetooth server and starts listening to clients.
"""
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(29, GPIO.OUT)
GPIO.output(29, GPIO.HIGH)
GPIO.setup(15, GPIO.IN, pull_up_down=GPIO.PUD_UP)
Thread(target=button_thread).start()
signal.signal(signal.SIGINT, signal_handler)
global net_interface
global wifi_on
global hotspot_enabled
global hotspot_ssid
global hotspot_pswd
global server_sock
global client_sock
global username
global type_pswd
global indiweb
global led_thread_run
global config_file
if len(sys.argv) == 5:
username = sys.argv[1]
print("Username = "******"Network interface = " + net_interface)
hotspot_ssid = sys.argv[3]
print("Hotspot SSID = " + hotspot_ssid)
hotspot_pswd = sys.argv[4]
print("Hotspot Password = "******"b9029ed0-6d6a-4ff6-b318-215067a6d8b1"
print("BT service UUID = " + uuid)
config_file = "{0}.{1}".format("/home" + username + "/", "Telescope-Pi.ini")
if username == "root":
if os.path.isdir("/home/pi/"):
config_file = "/home/pi/.Telescope-Pi.ini"
else:
config_file = None
print("Error! Invalid user!")
print("Running in emergency mode!")
emergency_mode_led()
config = configparser.ConfigParser()
if os.path.exists(config_file) and os.path.isfile(config_file):
config.read(config_file)
if "Wi-Fi" in config and config["Wi-Fi"] == "yes":
turn_on_wifi()
try:
if len(Cell.all(net_interface)) == 0:
print("No Wi-Fi networks found, starting hotspot.")
start_hotspot()
except InterfaceError as e:
print("An error occurred while scanning Wi-Fi network!")
print(str(e))
if hotspot_enabled is False and "Hotspot" in config and config["Hotspot"] == "yes":
start_hotspot()
if "INDI" in config and config["INDI"] == "yes":
indiweb_start()
else:
turn_on_wifi()
try:
if len(list(Cell.all(net_interface))) == 0:
print("No Wi-Fi networks found, starting hotspot.")
start_hotspot()
except InterfaceError as e:
print("An error occurred while scanning Wi-Fi network!")
print(str(e))
indiweb_start()
try:
server_sock = BluetoothSocket(RFCOMM)
server_sock.bind(("", PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
advertise_service(server_sock, "Telescope-Pi", service_id=uuid, service_classes=[
uuid, SERIAL_PORT_CLASS], profiles=[SERIAL_PORT_PROFILE])
Thread(target=led_thread).start()
while True:
print("Waiting for connection on RFCOMM channel %d" % port)
led_thread_run = True
client_sock, client_info = server_sock.accept()
led_thread_run = False
print("Accepted connection from " + str(client_info))
bt_send("NetInterface=" + net_interface +
"\nWiFi=" + str(wifi_on) +
"\nHotspot=" + str(hotspot_enabled) +
"\nHotspotSSID=" + hotspot_ssid +
"\nHotspotPswdType=WPA\nHotspotPswd=" + hotspot_pswd +
"\nINDI=" + str(indiweb is not None))
send_ip()
try:
while True:
data = client_sock.recv(1024)
if len(data) == 0:
break
for line in data.decode().splitlines():
parse_rfcomm(line.strip())
except Exception as e:
print(str(e))
print("Disconnected")
client_sock.close()
client_sock = None
type_pswd = False
except BluetoothError as e:
print("No Bluetooth adapter found! Make sure the systemd service has \"Type=idle\".")
print("Error message: " + str(e))
print("Running in emergency mode!")
emergency_mode_led()
else:
print("Usage: \"sudo python hotspot-controller-bluetooth.py" +
"<user> <network_interface> <hotspot_ssid> <hotspot_password>\"")
print("Running in emergency mode!")
emergency_mode_led()
class Networkable:
def __init__(self, is_server: bool = True):
self.is_server = is_server
self._threads = {}
self._connections = {}
self.socket = BluetoothSocket(RFCOMM)
self.socket_is_ready = False
self.socket_thread = self.spawn_thread("Accepting", self._accept,
True).start()
def connect(self):
while True:
if self.is_server:
self.socket.bind(("", PORT_ANY))
self.socket.listen(8)
_util.AdvertiseService(True, self.socket)
self.socket_is_ready = True
return
else:
found = _util.FindValidDevices(False)
if len(found) >= 1:
for address in found.keys():
try:
host, port = address.split("~")
self.socket.connect((host, int(port)))
self.socket_is_ready = True
return
except Exception:
continue
else:
_logging.Log(_logging.LogType.Info,
"Unable to find a server!", False).post()
def _accept(self):
if not self.socket_is_ready or self.socket is None: return
sock, addr = self.socket.accept()
self.save_connection(sock, addr)
def recieve(self, connection: object = None, data: str = None):
raise NotImplementedError()
def spawn_thread(self,
name: str = None,
target=None,
loop: bool = False,
args=tuple(),
kwargs={}):
if self._threads is None: return None
self.close_thread(name)
T = _threading.SimpleThread(target, loop, args, kwargs)
self._threads[name] = T
return T
def close_thread(self, name: str = None):
if self.thread_exists(name) and self._threads is not None:
T = self._threads[name]
del self._threads[name]
T.stop()
return True
else:
return False
def thread_exists(self, name: str = None):
return name in self._threads
def save_connection(self,
socket: BluetoothSocket = None,
addr: str = None):
if self._connections is None: return
c = ConnectionHandle(addr, socket, self.recieve)
self._connections[addr] = c
return c
def close_connection(self, addr: str = None):
if self.has_connection(addr) and self._connections is not None:
connection = self._connections[addr]
del self._connections[addr]
connection.close()
return True
else:
return False
def has_connection(self, addr: str = None):
return addr in self._connections and not self._connections[addr].closed
def __del__(self):
# Close connections and threads
for thread in self._threads.values():
thread.stop()
for connection in self._connections.values():
connection.close()
self._threads = None
self._connections = None
# Close socket related items
self.socket_is_ready = False
self.socket.close()
self.socket = None
class BTKbDevice():
"""Bluetooth HID keyboard device"""
# control and interrupt service ports
P_CTRL = 17 # Service port (control) from SDP record
P_INTR = 19 # Service port (interrupt) from SDP record
# D-Bus path of the BlueZ profile
PROFILE_DBUS_PATH = "/bluez/syss/btkbd_profile"
# file path of the SDP record
SDP_RECORD_PATH = "{}{}".format(sys.path[0], "/sdp_record.xml")
# device UUID
UUID = "00001124-0000-1000-8000-00805f9b34fb"
def __init__(self):
"""Initialize Bluetooth keyboard device"""
# read config file with address and device name
print("[*] Read configuration file")
config = configparser.ConfigParser()
config.read("../keyboard.conf")
try:
self.bdaddr = config['default']['BluetoothAddress']
self.device_name = config['default']['DeviceName']
self.device_short_name = config['default']['DeviceShortName']
self.interface = config['default']['Interface']
self.spoofing_method = config['default']['SpoofingMethod']
self.auto_connect = config['auto_connect']['AutoConnect']
self.connect_target = config['auto_connect']['Target']
except KeyError:
sys.exit("[-] Could not read all required configuration values")
print("[*] Initialize Bluetooth device")
self.configure_device()
self.register_bluez_profile()
def configure_device(self):
"""Configure bluetooth hardware device"""
print("[*] Configuring emulated Bluetooth keyboard")
# power on Bluetooth device
p = subprocess.run(
['btmgmt', '--index', self.interface, 'power', 'off'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# spoof device address if configured
if self.spoofing_method == 'bdaddr':
print("[+] Spoof device {} address {} via btmgmt".format(
self.interface, self.bdaddr))
# power on Bluetooth device
p = subprocess.run(
['btmgmt', '--index', self.interface, 'power', 'on'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set Bluetooth address using bdaddr software tool with manual
# reset, so that we have to power on the device
p = subprocess.run(
['bdaddr', '-i', self.interface, '-r', self.bdaddr],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# power on Bluetooth device
p = subprocess.run(
['btmgmt', '--index', self.interface, 'power', 'on'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set device class
print("[+] Set device class")
p = subprocess.run(
['btmgmt', '--index', self.interface, 'class', '5', '64'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set device name and short name
print("[+] Set device name: {} ({})".format(
self.device_name, self.device_short_name))
p = subprocess.run([
'btmgmt', '--index', self.interface, 'name', self.device_name,
self.device_short_name
],
stdout=subprocess.PIPE)
# set device to connectable
p = subprocess.run(
['btmgmt', '--index', self.interface, 'connectable', 'on'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# power on Bluetooth device
p = subprocess.run(
['btmgmt', '--index', self.interface, 'power', 'on'],
stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
elif self.spoofing_method == 'btmgmt':
print("[+] Spoof device {} address {} via btmgmt".format(
self.interface, self.bdaddr))
# set Bluetooth address
print("[+] Set Bluetooth address: {}".format(self.bdaddr))
p = subprocess.run([
'btmgmt', '--index', self.interface, 'public-addr', self.bdaddr
],
stdout=subprocess.PIPE)
print(p.stdout)
if "fail" in str(p.stdout, "utf-8"):
print("[-] Error setting Bluetooth address")
sys.exit(1)
# power on Bluetooth device using btmgmt software tool
p = subprocess.run(
['btmgmt', '--index', self.interface, 'power', 'on'],
stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device class
print("[+] Set device class")
p = subprocess.run(
['btmgmt', '--index', self.interface, 'class', '5', '64'],
stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device name and short name
print("[+] Set device name: {} ({})".format(
self.device_name, self.device_short_name))
p = subprocess.run([
'btmgmt', '--index', self.interface, 'name', self.device_name,
self.device_short_name
],
stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device to connectable
p = subprocess.run(
['btmgmt', '--index', self.interface, 'connectable', 'on'],
stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# turn on discoverable mode
print("[+] Turn on discoverable mode")
p = subprocess.run(['bluetoothctl', 'discoverable', 'on'],
stdout=subprocess.PIPE)
# print(p.stdout)
def register_bluez_profile(self):
"""Setup and register BlueZ profile"""
print("Configuring Bluez Profile")
# setup profile options
service_record = self.read_sdp_service_record()
opts = {
"ServiceRecord": service_record,
"Role": "server",
"RequireAuthentication": False,
"RequireAuthorization": False
}
# retrieve a proxy for the bluez profile interface
bus = dbus.SystemBus()
manager = dbus.Interface(bus.get_object("org.bluez", "/org/bluez"),
"org.bluez.ProfileManager1")
profile = BTKbdBluezProfile(bus, BTKbDevice.PROFILE_DBUS_PATH)
manager.RegisterProfile(BTKbDevice.PROFILE_DBUS_PATH, BTKbDevice.UUID,
opts)
print("[*] Profile registered")
def read_sdp_service_record(self):
"""Read SDP service record"""
print("[*] Reading service record")
try:
fh = open(BTKbDevice.SDP_RECORD_PATH, "r")
except Exception:
sys.exit("[*] Could not open the SDP record. Exiting ...")
return fh.read()
def listen(self):
"""Listen for incoming client connections"""
print("[*] Waiting for connections")
self.scontrol = BluetoothSocket(L2CAP)
self.sinterrupt = BluetoothSocket(L2CAP)
# bind these sockets to a port - port zero to select next available
self.scontrol.bind((self.bdaddr, self.P_CTRL))
self.sinterrupt.bind((self.bdaddr, self.P_INTR))
# start listening on the server sockets (only allow 1 connection)
self.scontrol.listen(1)
self.sinterrupt.listen(1)
self.ccontrol, cinfo = self.scontrol.accept()
print("[*] Connection on the control channel from {}".format(cinfo[0]))
self.cinterrupt, cinfo = self.sinterrupt.accept()
print("[*] Connection on the interrupt channel from {}".format(
cinfo[0]))
def connect(self, target):
"""Connect to target MAC (the keyboard must already be known to the
target)"""
print("[*] Connecting to {}".format(target))
self.scontrol = BluetoothSocket(L2CAP)
self.sinterrupt = BluetoothSocket(L2CAP)
self.scontrol.connect((target, self.P_CTRL))
self.sinterrupt.connect((target, self.P_INTR))
self.ccontrol = self.scontrol
self.cinterrupt = self.sinterrupt
def send_string(self, message):
"""Send a string to the host machine"""
self.cinterrupt.send(message)
from bluetooth import BluetoothSocket, RFCOMM, PORT_ANY, advertise_service, SERIAL_PORT_CLASS, SERIAL_PORT_PROFILE
server_sock = BluetoothSocket(RFCOMM)
server_sock.bind(("", PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
uuid = "a140ff7b-6539-40fe-9481-671108c144d6"
advertise_service(server_sock, "PenPi Service", uuid)
print("Waiting for connection on RFCOMM channel %d" % port)
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)
try:
while True:
data = client_sock.recv(1024)
if len(data) == 0: break
print("received [%s]" % data)
except IOError:
pass
print("disconnected")
client_sock.close()
server_sock.close()
class BluetoothInterface(BaseInterface):
"""The Bluetooth interface definition for communication with wireless client devices."""
__interface_name__ = 'bluetooth'
def __init__(self, controller):
"""
Initializes a bluetooth service that may be consumed by a remote client.
Arguments
---------
controller : controllers.base.BaseController
the parent controller that is instantiated this interface
"""
super(BluetoothInterface, self).__init__()
self.controller = controller
self.client_sock = None
self.client_info = None
self.rfcomm_channel = None
self.service_name = self.get_setting('service_name')
self.service_uuid = self.get_setting('service_uuid')
self.server_sock = None
self.thread = None
def connect(self):
"""Creates a new thread that listens for an incoming bluetooth RFCOMM connection."""
LOGGER.info('creating thread for bluetooth interface...')
self.thread = threading.Thread(target=self.listen_for_rfcomm_connection)
self.thread.daemon = True
self.thread.start()
def listen_for_rfcomm_connection(self):
"""
Starts bluetooth interfaces
"""
# prepare bluetooth server
self.server_sock = BluetoothSocket(RFCOMM)
self.server_sock.bind(("", PORT_ANY))
self.server_sock.listen(1)
self.rfcomm_channel = self.server_sock.getsockname()[1]
# start listening for incoming connections
try:
advertise_service(
sock=self.server_sock,
name=self.service_name,
service_id=self.service_uuid,
service_classes=[self.service_uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE]
)
except:
LOGGER.exception("[ERROR] failed to advertise service")
return
LOGGER.info('waiting for connection on RFCOMM channel %d', self.rfcomm_channel)
# accept received connection
self.client_sock, self.client_info = self.server_sock.accept()
self.state = self.__states__.STATE_CONNECTED
LOGGER.info('accepted connection from %r', self.client_info)
# start listening for data
self.consume_bus()
def disconnect(self):
"""
Closes Bluetooth connection and resets handle
"""
LOGGER.info('destroying bluetooth interface...')
self.state = self.__states__.STATE_DISCONNECTING
if self.client_sock and hasattr(self.client_sock, 'close'):
self.client_sock.close()
self.client_sock = None
if self.server_sock and hasattr(self.server_sock, 'close'):
self.server_sock.close()
self.server_sock = None
# reset the bluetooth interface
self.thread = None
self.perform_hci0_reset()
self.state = self.__states__.STATE_READY
@staticmethod
def perform_hci0_reset():
"""Resets the bluetooth hci0 device via hciconfig command line interface."""
try:
LOGGER.info('performing hci0 down/up...')
subprocess.Popen('hciconfig hci0 down', shell=True).communicate()
subprocess.Popen('hciconfig hci0 up', shell=True).communicate()
LOGGER.info('hci0 down/up has completed')
except Exception as exception:
LOGGER.exception("Failed to restart hci0 - %r", exception)
def receive(self, data):
"""
Processes received data from Bluetooth socket
Arguments
---------
data : basestring
the data received from the bluetooth connection
"""
try:
packet = json.loads(data.decode('utf-8'))
LOGGER.info('received packet via bluetooth: %r', packet['data'])
# invoke bound method (if set)
if self.receive_hook and hasattr(self.receive_hook, '__call__'):
self.receive_hook(packet['data'])
except Exception as exception:
LOGGER.exception('error: %r', exception)
def send(self, data):
"""
Sends data via Bluetooth socket connection
Arguments
---------
data : basestring
the data to be sent via this interface
"""
if self.state != self.__states__.STATE_CONNECTED:
LOGGER.error('error: send() was called but state is not connected')
return False
try:
LOGGER.info('sending IBUSPacket(s)...')
packets = []
for packet in data:
packets.append(packet.as_serializable_dict())
# encapsulate ibus packets and send
data = {"data": json.dumps(packets)} # TODO : is an inner json.dumps necessary?
LOGGER.info(data)
self.client_sock.send(json.dumps(data))
except Exception:
# socket was closed, graceful restart
LOGGER.exception('bluetooth send exception')
self.reconnect()
def consume_bus(self):
"""
Start listening for incoming data
"""
try:
LOGGER.info('starting to listen for bluetooth data...')
read_buffer_length = self.get_setting('read_buffer_length', int)
while self.client_sock:
data = self.client_sock.recv(read_buffer_length)
if len(data) > 0:
self.receive(data)
except Exception as exception:
LOGGER.exception('android device was disconnected - %r', exception)
self.reconnect()
class HID:
def __init__(self, bdaddress=None):
self.cport = 0x11 # HID's control PSM
self.iport = 0x13 # HID' interrupt PSM
self.backlog = 1
self.address = ""
if bdaddress:
self.address = bdaddress
# create the HID control socket
self.csock = BluetoothSocket(L2CAP)
self.csock.bind((self.address, self.cport))
set_l2cap_mtu(self.csock, 64)
self.csock.settimeout(2)
self.csock.listen(self.backlog)
# create the HID interrupt socket
self.isock = BluetoothSocket(L2CAP)
self.isock.bind((self.address, self.iport))
set_l2cap_mtu(self.isock, 64)
self.isock.settimeout(2)
self.isock.listen(self.backlog)
self.connected = False
self.client_csock = None
self.caddress = None
self.client_isock = None
self.iaddress = None
def listen(self):
try:
(self.client_csock, self.caddress) = self.csock.accept()
print("Accepted Control connection from %s" % self.caddress[0])
(self.client_isock, self.iaddress) = self.isock.accept()
print("Accepted Interrupt connection from %s" % self.iaddress[0])
self.connected = True
return True
except Exception as _:
self.connected = False
return False
@staticmethod
def get_local_address():
hci = BluetoothSocket(HCI)
fd = hci.fileno()
buf = array.array('B', [0] * 96)
ioctl(fd, _bluetooth.HCIGETDEVINFO, buf, 1)
data = struct.unpack_from("H8s6B", buf.tostring())
return data[2:8][::-1]
def get_control_socket(self):
if self.connected:
return self.client_csock, self.caddress
else:
return None
def get_interrupt_socket(self):
if self.connected:
return self.client_isock, self.iaddress
else:
return None
def run_bt_server(end_event, device_name, account):
"""Adapted from: https://github.com/EnableTech/raspberry-bluetooth-demo"""
server_sock = BluetoothSocket(RFCOMM)
server_sock.bind(("", PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
print("listening on port %d" % port)
uuid = "1e0ca4ea-299d-4335-93eb-27fcfe7fa848"
try:
advertise_service(
server_sock,
device_name,
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE],
# protocols=[OBEX_UUID],
)
except BluetoothError as e:
print('ERROR: Bluetooth Error:{}.\n Quiting bluetooth server. Is your '
'bluetooth setup correctly? Do you have sudo privileges?'.format(
e))
return
print("Waiting for connection on RFCOMM channel %d" % port)
client_sock, client_info = server_sock.accept()
print("Accepted connection from ", client_info)
client_sock.setblocking(0)
bufferdata = bytearray()
try:
while not end_event.is_set():
ready = select.select([client_sock], [], [], 5)
if not ready[0]:
break
data = client_sock.recv(1024)
if len(data) == 0:
break
print("received [%s]" % data)
bufferdata = bufferdata + data
print(bufferdata)
try:
end = bufferdata.index(b'\n')
our_data = bufferdata[0:end].strip(b'\r')
if len(bufferdata) > end:
bufferdata = bufferdata[end + 1:]
else:
bufferdata = bytearray()
bluetooth_process(our_data, client_sock, account)
except ValueError:
pass
except IOError:
pass
print("bluetooth server disconnected")
client_sock.close()
server_sock.close()
print("all done")
class BluetoothConn(ServerInterface):
def __init__(self, config: ProjectConfig):
self.conn = None
self.client = None
self._connected = False
self.config = config
self.address = self.config.get('BT_ADDRESS')
self.port = int(self.config.get('BT_PORT'))
def get_name(self) -> str:
return format(
f'Bluetooth connection on {self.address} port {self.port}')
def get_tags(self) -> dict:
return {'ANDROID': True, 'BT': True}
def is_connected(self) -> bool:
return self._connected
def connect(self):
try:
self.conn = BluetoothSocket(
bluetooth.RFCOMM) # use RFCOMM protocol
self.conn.bind(
(self.address,
self.port)) # empty address; use any available adapter
self.address, self.port = self.conn.getsockname()
self.conn.listen(1)
uuid = self.config.get('BT_UUID')
bluetooth.advertise_service(
sock=self.conn,
name='MDP-Group-15-Bluetooth-Server',
service_id=uuid,
service_classes=[uuid, bluetooth.SERIAL_PORT_CLASS],
profiles=[bluetooth.SERIAL_PORT_PROFILE],
)
print(
f'Listening for Bluetooth connection on {self.address} port {self.port}'
)
self.client, client_info = self.conn.accept()
print(f'Connected to {client_info}')
self._connected = True
except Exception as e:
print(f'Error with {self.get_name()}: {e}')
self.disconnect()
raise ConnectionError
def read(self):
try:
data = self.client.recv(1024)
# data = data.decode('utf-8')
if not data:
raise ConnectionError('No transmission')
data_dict = json.loads(data)
print(f'Received from Android device: {data}')
return self.format_data(data_dict)
except Exception as e:
print(f'Error with reading from {self.get_name()}: {e}')
#print('Reconnecting...')
#self.disconnect()
#raise ConnectionError
def write(self, message):
try:
json_str = json.dumps(message)
byte_msg = bytes(json_str, encoding='utf-8')
self.client.send(byte_msg)
print(f'Sent to Android device: {byte_msg}')
except Exception as e:
print(f'Error with writing {message} to {self.get_name()}: {e}')
#print('Reconnecting...')
#self.disconnect()
raise ConnectionError
def disconnect(self):
if self.conn:
self.conn.close()
print('Terminating server socket..')
if self.client:
self.client.close()
print('Terminating client socket..')
self._connected = False
class BTKbDevice():
"""Bluetooth HID keyboard device"""
# control and interrupt service ports
P_CTRL = 17 # Service port (control) from SDP record
P_INTR = 19 # Service port (interrupt) from SDP record
# D-Bus path of the BlueZ profile
PROFILE_DBUS_PATH = "/bluez/syss/btkbd_profile"
# file path of the SDP record
SDP_RECORD_PATH = "{}{}".format(sys.path[0], "/sdp_record.xml")
# device UUID
UUID = "00001124-0000-1000-8000-00805f9b34fb"
def __init__(self):
"""Initialize Bluetooth keyboard device"""
# read config file with address and device name
print("[*] Read configuration file")
config = configparser.ConfigParser()
config.read("../keyboard.conf")
try:
self.bdaddr = config['default']['BluetoothAddress']
self.device_name = config['default']['DeviceName']
self.device_short_name = config['default']['DeviceShortName']
self.interface = config['default']['Interface']
self.spoofing_method = config['default']['SpoofingMethod']
self.auto_connect = config['auto_connect']['AutoConnect']
self.connect_target = config['auto_connect']['Target']
except KeyError:
sys.exit("[-] Could not read all required configuration values")
print("[*] Initialize Bluetooth device")
self.configure_device()
self.register_bluez_profile()
def configure_device(self):
"""Configure bluetooth hardware device"""
print("[*] Configuring emulated Bluetooth keyboard")
# power on Bluetooth device
p = subprocess.run(['btmgmt', '--index', self.interface, 'power',
'off'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# spoof device address if configured
if self.spoofing_method == 'bdaddr':
print("[+] Spoof device {} address {} via btmgmt".
format(self.interface, self.bdaddr))
# power on Bluetooth device
p = subprocess.run(['btmgmt', '--index', self.interface, 'power',
'on'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set Bluetooth address using bdaddr software tool with manual
# reset, so that we have to power on the device
p = subprocess.run(['bdaddr', '-i', self.interface, '-r',
self.bdaddr], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# power on Bluetooth device
p = subprocess.run(['btmgmt', '--index', self.interface, 'power',
'on'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set device class
print("[+] Set device class")
p = subprocess.run(['btmgmt', '--index', self.interface, 'class',
'5', '64'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# set device name and short name
print("[+] Set device name: {} ({})".
format(self.device_name, self.device_short_name))
p = subprocess.run(['btmgmt', '--index', self.interface, 'name',
self.device_name, self.device_short_name],
stdout=subprocess.PIPE)
# set device to connectable
p = subprocess.run(['btmgmt', '--index', self.interface,
'connectable', 'on'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
# power on Bluetooth device
p = subprocess.run(['btmgmt', '--index', self.interface, 'power',
'on'], stdout=subprocess.PIPE)
time.sleep(OS_CMD_SLEEP)
elif self.spoofing_method == 'btmgmt':
print("[+] Spoof device {} address {} via btmgmt".
format(self.interface, self.bdaddr))
# set Bluetooth address
print("[+] Set Bluetooth address: {}".format(self.bdaddr))
p = subprocess.run(['btmgmt', '--index', self.interface,
'public-addr', self.bdaddr],
stdout=subprocess.PIPE)
print(p.stdout)
if "fail" in str(p.stdout, "utf-8"):
print("[-] Error setting Bluetooth address")
sys.exit(1)
# power on Bluetooth device using btmgmt software tool
p = subprocess.run(['btmgmt', '--index', self.interface, 'power',
'on'], stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device class
print("[+] Set device class")
p = subprocess.run(['btmgmt', '--index', self.interface, 'class',
'5', '64'], stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device name and short name
print("[+] Set device name: {} ({})".
format(self.device_name, self.device_short_name))
p = subprocess.run(['btmgmt', '--index', self.interface, 'name',
self.device_name, self.device_short_name],
stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# set device to connectable
p = subprocess.run(['btmgmt', '--index', self.interface,
'connectable', 'on'], stdout=subprocess.PIPE)
# print(p.stdout)
time.sleep(OS_CMD_SLEEP)
# turn on discoverable mode
print("[+] Turn on discoverable mode")
p = subprocess.run(['bluetoothctl', 'discoverable', 'on'],
stdout=subprocess.PIPE)
# print(p.stdout)
def register_bluez_profile(self):
"""Setup and register BlueZ profile"""
print("Configuring Bluez Profile")
# setup profile options
service_record = self.read_sdp_service_record()
opts = {
"ServiceRecord": service_record,
"Role": "server",
"RequireAuthentication": False,
"RequireAuthorization": False
}
# retrieve a proxy for the bluez profile interface
bus = dbus.SystemBus()
manager = dbus.Interface(bus.get_object("org.bluez", "/org/bluez"),
"org.bluez.ProfileManager1")
profile = BTKbdBluezProfile(bus, BTKbDevice.PROFILE_DBUS_PATH)
manager.RegisterProfile(BTKbDevice.PROFILE_DBUS_PATH, BTKbDevice.UUID,
opts)
print("[*] Profile registered")
def read_sdp_service_record(self):
"""Read SDP service record"""
print("[*] Reading service record")
try:
fh = open(BTKbDevice.SDP_RECORD_PATH, "r")
except Exception:
sys.exit("[*] Could not open the SDP record. Exiting ...")
return fh.read()
def listen(self):
"""Listen for incoming client connections"""
print("[*] Waiting for connections")
self.scontrol = BluetoothSocket(L2CAP)
self.sinterrupt = BluetoothSocket(L2CAP)
# bind these sockets to a port - port zero to select next available
self.scontrol.bind((self.bdaddr, self.P_CTRL))
self.sinterrupt.bind((self.bdaddr, self.P_INTR))
# start listening on the server sockets (only allow 1 connection)
self.scontrol.listen(1)
self.sinterrupt.listen(1)
self.ccontrol, cinfo = self.scontrol.accept()
print("[*] Connection on the control channel from {}"
.format(cinfo[0]))
self.cinterrupt, cinfo = self.sinterrupt.accept()
print("[*] Connection on the interrupt channel from {}"
.format(cinfo[0]))
def connect(self, target):
"""Connect to target MAC (the keyboard must already be known to the
target)"""
print("[*] Connecting to {}".format(target))
self.scontrol = BluetoothSocket(L2CAP)
self.sinterrupt = BluetoothSocket(L2CAP)
self.scontrol.connect((target, self.P_CTRL))
self.sinterrupt.connect((target, self.P_INTR))
self.ccontrol = self.scontrol
self.cinterrupt = self.sinterrupt
def send_string(self, message):
"""Send a string to the host machine"""
self.cinterrupt.send(message)
class Hal(object):
# usedSensors is unused, the code-generator for lab.openroberta > 1.4 wont
# pass it anymore
def __init__(self, brickConfiguration, usedSensors=None):
self.cfg = brickConfiguration
dir = os.path.dirname(__file__)
self.font_s = ImageFont.load(os.path.join(dir, 'ter-u12n_unicode.pil'))
self.font_x = ImageFont.load(os.path.join(dir, 'ter-u18n_unicode.pil'))
self.lcd = ev3dev.Screen()
self.led = ev3dev.Leds
self.keys = ev3dev.Button()
self.sound = ev3dev.Sound
(self.font_w, self.font_h) = self.lcd.draw.textsize('X',
font=self.font_s)
self.timers = {}
self.sys_bus = None
self.bt_server = None
self.bt_connections = []
# factory methods
@staticmethod
def makeLargeMotor(port, regulated, direction, side):
try:
m = ev3dev.LargeMotor(port)
if direction is 'backward':
m.polarity = 'inversed'
else:
m.polarity = 'normal'
except (AttributeError, OSError):
logger.info('no large motor connected to port [%s]' % port)
logger.exception("HW Config error")
m = None
return m
@staticmethod
def makeMediumMotor(port, regulated, direction, side):
try:
m = ev3dev.MediumMotor(port)
if direction is 'backward':
m.polarity = 'inversed'
else:
m.polarity = 'normal'
except (AttributeError, OSError):
logger.info('no medium motor connected to port [%s]' % port)
logger.exception("HW Config error")
m = None
return m
@staticmethod
def makeColorSensor(port):
try:
s = ev3dev.ColorSensor(port)
except (AttributeError, OSError):
logger.info('no color sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeGyroSensor(port):
try:
s = ev3dev.GyroSensor(port)
except (AttributeError, OSError):
logger.info('no gyro sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeI2cSensor(port):
try:
s = ev3dev.I2cSensor(port)
except (AttributeError, OSError):
logger.info('no i2c sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeInfraredSensor(port):
try:
s = ev3dev.InfraredSensor(port)
except (AttributeError, OSError):
logger.info('no infrared sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeLightSensor(port):
try:
s = ev3dev.LightSensor(port)
except (AttributeError, OSError):
logger.info('no light sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeSoundSensor(port):
try:
s = ev3dev.SoundSensor(port)
except (AttributeError, OSError):
logger.info('no sound sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeTouchSensor(port):
try:
s = ev3dev.TouchSensor(port)
except (AttributeError, OSError):
logger.info('no touch sensor connected to port [%s]' % port)
s = None
return s
@staticmethod
def makeUltrasonicSensor(port):
try:
s = ev3dev.UltrasonicSensor(port)
except (AttributeError, OSError):
logger.info('no ultrasonic sensor connected to port [%s]' % port)
s = None
return s
# control
def waitFor(self, ms):
time.sleep(ms / 1000.0)
def busyWait(self):
'''Used as interrupptible busy wait.'''
time.sleep(0.0)
# lcd
def drawText(self, msg, x, y, font=None):
font = font or self.font_s
self.lcd.draw.text((x * self.font_w, y * self.font_h), msg, font=font)
self.lcd.update()
def drawPicture(self, picture, x, y):
# logger.info('len(picture) = %d', len(picture))
size = (178, 128)
# One image is supposed to be 178*128/8 = 2848 bytes
if len(picture) < 20:
# deprecated server API
# TODO(ensonic): remove after ora-2.1.4 is online
from roberta.StaticData import IMAGES
if not hasattr(self, 'images'):
self.images = {}
if picture not in self.images:
self.images[picture] = Image.frombytes('1', size,
IMAGES[picture], 'raw',
'1;IR', 0, 1)
pixels = self.images[picture]
else:
# string data is in utf-16 format and padding with extra 0 bytes
data = bytes(picture, 'utf-16')[::2]
pixels = Image.frombytes('1', size, data, 'raw', '1;IR', 0, 1)
self.lcd.image.paste(pixels, (x, y))
self.lcd.update()
def clearDisplay(self):
self.lcd.clear()
self.lcd.update()
# led
def ledOn(self, color, mode):
# color: green, red, orange - LED.COLOR.{RED,GREEN,AMBER}
# mode: on, flash, double_flash
if mode is 'on':
if color is 'green':
self.led.set_color(ev3dev.Leds.LEFT, ev3dev.Leds.GREEN)
self.led.set_color(ev3dev.Leds.RIGHT, ev3dev.Leds.GREEN)
elif color is 'red':
self.led.set_color(ev3dev.Leds.LEFT, ev3dev.Leds.RED)
self.led.set_color(ev3dev.Leds.RIGHT, ev3dev.Leds.RED)
elif color is 'orange':
self.led.set_color(ev3dev.Leds.LEFT, ev3dev.Leds.ORANGE)
self.led.set_color(ev3dev.Leds.RIGHT, ev3dev.Leds.ORANGE)
elif mode in ['flash', 'double_flash']:
# FIXME: timer mode does not support double flash
group = []
if color in ['green', 'orange']:
group.append(self.led.green_left)
group.append(self.led.green_right)
if color in ['red', 'orange']:
group.append(self.led.red_left)
group.append(self.led.red_right)
self.led.set(group, trigger='timer')
# when the trigger attribute is set other attributes appear
# dynamically :/ - but this still does not help :/
for i in range(5):
try:
self.led.set(group, delay_on=200, delay_off=800)
break
except IOError as e:
logger.info('failed to set blink timing [%s]' % e.message)
time.sleep(0.1)
def ledOff(self):
self.led.all_off()
def resetLED(self):
self.ledOff()
# key
def isKeyPressed(self, key):
if key in ['any', '*']:
return self.keys.any()
else:
# remap some keys
key_aliases = {
'escape': 'backspace',
'back': 'backspace',
}
if key in key_aliases:
key = key_aliases[key]
return key in self.keys.buttons_pressed
def isKeyPressedAndReleased(self, key):
return False
# tones
def playTone(self, frequency, duration):
# this is already handled by the sound api (via beep cmd)
# frequency = frequency if frequency >= 100 else 0
self.sound.tone(frequency, duration).wait()
def playFile(self, systemSound):
# systemSound is a enum for preset beeps:
# http://www.lejos.org/ev3/docs/lejos/hardware/Audio.html#systemSound-int-
# http://sf.net/p/lejos/ev3/code/ci/master/tree/ev3classes/src/lejos/remote/nxt/RemoteNXTAudio.java#l20
C2 = 523
if systemSound == 0:
self.playTone(600, 200)
elif systemSound == 1:
self.sound.tone([(600, 150, 50), (600, 150, 50)]).wait()
elif systemSound == 2: # C major arpeggio
self.sound.tone([(C2 * i / 4, 50, 50) for i in range(4, 7)]).wait()
elif systemSound == 3:
self.sound.tone([(C2 * i / 4, 50, 50)
for i in range(7, 4, -1)]).wait()
elif systemSound == 4:
self.playTone(100, 500)
def setVolume(self, volume):
# FIXME: https://github.com/rhempel/ev3dev-lang-python/issues/258
# will be in python-ev3dev-0.9.0
try:
self.sound.set_volume(volume)
except AttributeError:
pass
self.sound.volume = volume
def getVolume(self):
# FIXME: https://github.com/rhempel/ev3dev-lang-python/issues/258
# will be in python-ev3dev-0.9.0
try:
return self.sound.get_volume()
except AttributeError:
pass
return self.sound.volume
# actors
# http://www.ev3dev.org/docs/drivers/tacho-motor-class/
def scaleSpeed(self, m, speed_pct):
return int(speed_pct * m.max_speed / 100.0)
def rotateRegulatedMotor(self, port, speed_pct, mode, value):
# mode: degree, rotations, distance
m = self.cfg['actors'][port]
speed = self.scaleSpeed(m, clamp(speed_pct, -100, 100))
if mode is 'degree':
m.run_to_rel_pos(position_sp=value, speed_sp=speed)
while (m.state):
self.busyWait()
elif mode is 'rotations':
value *= m.count_per_rot
m.run_to_rel_pos(position_sp=int(value), speed_sp=speed)
while (m.state):
self.busyWait()
def rotateUnregulatedMotor(self, port, speed_pct, mode, value):
speed_pct = clamp(speed_pct, -100, 100)
m = self.cfg['actors'][port]
if mode is 'rotations':
value *= m.count_per_rot
if speed_pct >= 0:
value = m.position + value
m.run_direct(duty_cycle_sp=int(speed_pct))
while (m.position < value):
self.busyWait()
else:
value = m.position - value
m.run_direct(duty_cycle_sp=int(speed_pct))
while (m.position > value):
self.busyWait()
m.stop()
def turnOnRegulatedMotor(self, port, value):
m = self.cfg['actors'][port]
m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100)))
def turnOnUnregulatedMotor(self, port, value):
value = clamp(value, -100, 100)
self.cfg['actors'][port].run_direct(duty_cycle_sp=int(value))
def setRegulatedMotorSpeed(self, port, value):
m = self.cfg['actors'][port]
# https://github.com/rhempel/ev3dev-lang-python/issues/263
# m.speed_sp = self.scaleSpeed(m, clamp(value, -100, 100))
m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100)))
def setUnregulatedMotorSpeed(self, port, value):
value = clamp(value, -100, 100)
self.cfg['actors'][port].duty_cycle_sp = int(value)
def getRegulatedMotorSpeed(self, port):
m = self.cfg['actors'][port]
return m.speed * 100.0 / m.max_speed
def getUnregulatedMotorSpeed(self, port):
return self.cfg['actors'][port].duty_cycle
def stopMotor(self, port, mode='float'):
# mode: float, nonfloat
# stop_actions: ['brake', 'coast', 'hold']
m = self.cfg['actors'][port]
if mode is 'float':
m.stop_action = 'coast'
elif mode is 'nonfloat':
m.stop_action = 'brake'
m.stop()
def stopMotors(self, left_port, right_port):
self.stopMotor(left_port)
self.stopMotor(right_port)
def stopAllMotors(self):
# [m for m in [Motor(port) for port in ['outA', 'outB', 'outC', 'outD']] if m.connected]
for file in glob.glob('/sys/class/tacho-motor/motor*/command'):
with open(file, 'w') as f:
f.write('stop')
def regulatedDrive(self, left_port, right_port, reverse, direction,
speed_pct):
# direction: forward, backward
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
if direction is 'backward':
speed_pct = -speed_pct
ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct))
mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct))
def driveDistance(self, left_port, right_port, reverse, direction,
speed_pct, distance):
# direction: forward, backward
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
if direction is 'backward':
dc = -dc
# set all attributes
ml.stop_action = 'brake'
ml.position_sp = int(dc * ml.count_per_rot)
ml.speed_sp = self.scaleSpeed(ml, speed_pct)
mr.stop_action = 'brake'
mr.position_sp = int(dc * mr.count_per_rot)
mr.speed_sp = self.scaleSpeed(mr, speed_pct)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
# logger.debug("driving: %s, %s" % (ml.state, mr.state))
while (ml.state or mr.state):
self.busyWait()
def rotateDirectionRegulated(self, left_port, right_port, reverse,
direction, speed_pct):
# direction: left, right
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
if direction is 'left':
mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct))
ml.run_forever(speed_sp=self.scaleSpeed(ml, -speed_pct))
else:
ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct))
mr.run_forever(speed_sp=self.scaleSpeed(mr, -speed_pct))
def rotateDirectionAngle(self, left_port, right_port, reverse, direction,
speed_pct, angle):
# direction: left, right
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
circ = math.pi * self.cfg['track-width']
distance = angle * circ / 360.0
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
logger.debug("doing %lf rotations" % dc)
# set all attributes
ml.stop_action = 'brake'
ml.speed_sp = self.scaleSpeed(ml, speed_pct)
mr.stop_action = 'brake'
mr.speed_sp = self.scaleSpeed(mr, speed_pct)
if direction is 'left':
mr.position_sp = int(dc * mr.count_per_rot)
ml.position_sp = int(-dc * ml.count_per_rot)
else:
ml.position_sp = int(dc * ml.count_per_rot)
mr.position_sp = int(-dc * mr.count_per_rot)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
logger.debug("turning: %s, %s" % (ml.state, mr.state))
while (ml.state or mr.state):
self.busyWait()
def driveInCurve(self,
direction,
left_port,
left_speed_pct,
right_port,
right_speed_pct,
distance=None):
# direction: foreward, backward
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
left_speed_pct = self.scaleSpeed(ml, clamp(left_speed_pct, -100, 100))
right_speed_pct = self.scaleSpeed(mr, clamp(right_speed_pct, -100,
100))
if distance:
speed_pct = (left_speed_pct + right_speed_pct) / 2.0
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
left_dc = dc * left_speed_pct / speed_pct
right_dc = dc * right_speed_pct / speed_pct
# set all attributes
ml.stop_action = 'brake'
ml.speed_sp = int(left_speed_pct)
mr.stop_action = 'brake'
mr.speed_sp = int(right_speed_pct)
if direction is 'backwards':
ml.position_sp = int(-left_dc * ml.count_per_rot)
mr.position_sp = int(-right_dc * mr.count_per_rot)
else:
ml.position_sp = int(left_dc * ml.count_per_rot)
mr.position_sp = int(right_dc * mr.count_per_rot)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
while (ml.state or mr.state):
self.busyWait()
else:
if direction is 'backwards':
ml.run_forever(speed_sp=int(-left_speed_pct))
mr.run_forever(speed_sp=int(-right_speed_pct))
else:
ml.run_forever(speed_sp=int(left_speed_pct))
mr.run_forever(speed_sp=int(right_speed_pct))
# sensors
def scaledValue(self, sensor):
return sensor.value() / (10.0**sensor.decimals)
# touch sensor
def isPressed(self, port):
return self.scaledValue(self.cfg['sensors'][port])
# ultrasonic sensor
def getUltraSonicSensorDistance(self, port):
s = self.cfg['sensors'][port]
s.mode = 'US-DIST-CM'
return self.scaledValue(s)
def getUltraSonicSensorPresence(self, port):
s = self.cfg['sensors'][port]
s.mode = 'US-SI-CM'
return self.scaledValue(s)
# gyro
# http://www.ev3dev.org/docs/sensors/lego-ev3-gyro-sensor/
def resetGyroSensor(self, port):
# change mode to reset for GYRO-ANG and GYRO-G&A
self.cfg['sensors'][port].mode = 'GYRO-RATE'
self.cfg['sensors'][port].mode = 'GYRO-ANG'
def getGyroSensorValue(self, port, mode):
# mode = rate, angle
s = self.cfg['sensors'][port]
if mode is 'angle':
s.mode = 'GYRO-ANG'
elif mode is 'rate':
s.mode = 'GYRO-RATE'
return self.scaledValue(s)
# color
# http://www.ev3dev.org/docs/sensors/lego-ev3-color-sensor/
def getColorSensorAmbient(self, port):
s = self.cfg['sensors'][port]
s.mode = 'COL-AMBIENT'
return self.scaledValue(s)
def getColorSensorColour(self, port):
colors = [
'none', 'black', 'blue', 'green', 'yellow', 'red', 'white', 'brown'
]
s = self.cfg['sensors'][port]
s.mode = 'COL-COLOR'
return colors[int(self.scaledValue(s))]
def getColorSensorRed(self, port):
s = self.cfg['sensors'][port]
s.mode = 'COL-REFLECT'
return self.scaledValue(s)
def getColorSensorRgb(self, port):
s = self.cfg['sensors'][port]
s.mode = 'RGB-RAW'
return (s.value(0), s.value(1), s.value(2))
# infrared
# http://www.ev3dev.org/docs/sensors/lego-ev3-infrared-sensor/
def getInfraredSensorSeek(self, port):
s = self.cfg['sensors'][port]
s.mode = 'IR-SEEK'
return self.scaledValue(s)
def getInfraredSensorDistance(self, port):
s = self.cfg['sensors'][port]
s.mode = 'IR-PROX'
return self.scaledValue(s)
# timer
def getTimerValue(self, timer):
if timer in self.timers:
return time.clock() - self.timers[timer]
else:
self.timers[timer] = time.clock()
def resetTimer(self, timer):
del self.timers[timer]
# tacho-motor position
def resetMotorTacho(self, actorPort):
m = self.cfg['actors'][actorPort]
m.position = 0
def getMotorTachoValue(self, actorPort, mode):
m = self.cfg['actors'][actorPort]
tachoCount = m.position
if mode == 'degree':
return tachoCount * 360.0 / float(m.count_per_rot)
elif mode in ['rotation', 'distance']:
rotations = float(tachoCount) / float(m.count_per_rot)
if mode == 'rotation':
return rotations
else:
distance = round(math.pi * self.cfg['wheel-diameter'] *
rotations)
return distance
else:
raise ValueError('incorrect MotorTachoMode: %s' % mode)
# communication
def establishConnectionTo(self, host):
# host can also be a name, resolving it is slow though and requires the
# device to be visible
if not bluetooth.is_valid_address(host):
nearby_devices = bluetooth.discover_devices()
for bdaddr in nearby_devices:
if host == bluetooth.lookup_name(bdaddr):
host = bdaddr
break
if bluetooth.is_valid_address(host):
con = BluetoothSocket(bluetooth.RFCOMM)
con.connect((host, 1)) # 0 is channel
self.bt_connections.append(con)
return len(self.bt_connections) - 1
else:
return -1
def waitForConnection(self):
# enable visibility
if not self.sys_bus:
self.sys_bus = dbus.SystemBus()
# do only once (since we turn off the timeout)
# alternatively set DiscoverableTimeout = 0 in /etc/bluetooth/main.conf
# and run hciconfig hci0 piscan, from robertalab initscript
hci0 = self.sys_bus.get_object('org.bluez', '/org/bluez/hci0')
props = dbus.Interface(hci0, 'org.freedesktop.DBus.Properties')
props.Set('org.bluez.Adapter1', 'DiscoverableTimeout', dbus.UInt32(0))
props.Set('org.bluez.Adapter1', 'Discoverable', True)
if not self.bt_server:
self.bt_server = BluetoothSocket(bluetooth.RFCOMM)
self.bt_server.bind(("", bluetooth.PORT_ANY))
self.bt_server.listen(1)
(con, info) = self.bt_server.accept()
self.bt_connections.append(con)
return len(self.bt_connections) - 1
def readMessage(self, con_ix):
message = "NO MESSAGE"
if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]:
try:
logger.debug('reading msg')
# TODO(ensonic): how much do we actually expect
# here is the lejos counter part
# https://github.com/OpenRoberta/robertalab-ev3lejos/blob/master/
# EV3Runtime/src/main/java/de/fhg/iais/roberta/runtime/ev3/BluetoothComImpl.java#L40..L59
message = self.bt_connections[con_ix].recv(128)
logger.debug('received msg [%s]' % message)
except bluetooth.btcommon.BluetoothError:
logger.exception("Bluetooth error")
self.bt_connections[con_ix] = None
return message
def sendMessage(self, con_ix, message):
if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]:
try:
logger.debug('sending msg [%s]' % message)
self.bt_connections[con_ix].send(message)
logger.debug('sent msg')
except bluetooth.btcommon.BluetoothError:
logger.exception("Bluetooth error")
self.bt_connections[con_ix] = None
def launch_hub_bluetooth_server(config_file_path):
'''
Launches the Tremium Hub bluetooth server which the Tremium Nodes connect to.
Parameters
----------
config_file_path (str) : path to the hub configuration file
'''
# loading Tremium Hub configurations
config_manager = HubConfigurationManager(config_file_path)
log_file_path = os.path.join(
config_manager.config_data["hub-file-transfer-dir"],
config_manager.config_data["bluetooth-server-log-name"])
# setting up logging
logger = logging.getLogger()
logger.setLevel(logging.INFO)
log_handler = logging.handlers.WatchedFileHandler(log_file_path)
log_handler.setFormatter(
logging.Formatter('%(name)s - %(levelname)s - %(message)s'))
logger.addHandler(log_handler)
# defining container for connection handler handles
connection_handlers_h = []
try:
# creating socket to listen for new connections
listener_s = BluetoothSocket()
listener_s.bind(
(config_manager.config_data["bluetooth-adapter-mac-server"],
config_manager.config_data["bluetooth-port"]))
listener_s.listen(1)
# advertising the listenning connection
advertise_service(listener_s, config_manager.config_data["hub-id"])
bind_address = listener_s.getsockname()
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - Hub Bluetooth server listening on address : {1}".format(
time_str, bind_address))
except Exception as e:
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - Hub Bluetooth server failed to create listener socket : {1}"
.format(time_str, e))
raise
while True:
try:
# blocking until a new connection occurs, then create connection handler
client_s, remote_address = listener_s.accept()
client_s.settimeout(
config_manager.config_data["bluetooth-comm-timeout"])
connection_handler = HubServerConnectionHandler(
config_file_path, client_s, remote_address)
# launching connection handler in a seperate process
process_h = Process(target=connection_handler.handle_connection,
args=())
process_h.start()
connection_handlers_h.append(process_h)
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.info(
"{0} - Hub Bluetooth server accepted and is handling connection from remote : {1}\
".format(time_str, remote_address))
#regular check to clear dead handles
for handler_h in connection_handlers_h:
if handler_h is not None:
connection_handlers_h.remove(handler_h)
except Exception as e:
# killing all connection handler processes still running
for handler_h in connection_handlers_h:
if handler_h.exitcode is None:
handler_h.terminate()
time_str = datetime.datetime.fromtimestamp(
time.time()).strftime('%Y-%m-%d_%H-%M-%S')
logging.error(
"{0} - Hub Bluetooth server failed to handle incoming connection : {1}"
.format(time_str, e))
raise
#!/usr/bin/env python3
# coding: utf-8
from bluetooth import BluetoothSocket
host_BD = 'B8:27:EB:A5:03:6C'
port = 3
backlog = 2
size = 1024
blueth = BluetoothSocket(bluetooth.RFCOMM)
blueth.bind((host_BD, port))
blueth.listen(backlog)
try:
client, client_info = blueth.accept()
while True:
data = client.recv(1024)
if data:
print(data)
except:
print("Closing socket")
client.close()
blueth.close()
class RfcommServer(object):
'''An RFComm Server designed to propagate bluetooth messages to UDP broadcasts.'''
def __init__(self, establish_bt=True):
self.folder = path.dirname(path.abspath(__file__))
try:
with open(path.join(self.folder,
'settings.yaml')) as settings_file:
self.settings = yaml.safe_load(settings_file)
except FileNotFoundError:
print(
"Create a settings.yaml file by copying settings-example.yaml")
raise
self.server_sock = None
self.client_sock = None
if establish_bt:
self.server_sock = BluetoothSocket(RFCOMM)
self.server_sock.bind(("", PORT_ANY))
self.server_sock.listen(1)
uuid = "28ccf815-245e-436f-a002-8e72a67422a8"
# doesn't appear to do anything on RasPi...
advertise_service(self.server_sock,
"LightNet",
service_id=uuid,
service_classes=[uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE])
self.broadcast_socket = socket.socket(socket.AF_INET,
socket.SOCK_DGRAM)
self.broadcast_socket.setsockopt(socket.SOL_SOCKET,
socket.SO_REUSEADDR, 1)
self.broadcast_socket.setsockopt(socket.SOL_SOCKET,
socket.SO_BROADCAST, 1)
def loop(self, gen=None):
'''Listen for and handle bluetooth commands.'''
if not gen:
gen = self.command_generator()
broadcast_dest = (self.settings['broadcast']['ip'],
self.settings['broadcast']['port'])
for command in gen:
print("received [{}]".format(command))
if command.startswith('#'):
self.execute_command(command[1:].strip())
continue
self.broadcast_socket.sendto(command.encode(), broadcast_dest)
self.respond('okay')
print("all done")
def execute_command(self, command):
'''Find and execute a command. '''
folder = path.join(self.folder, 'commands')
for filename in os.listdir(folder):
if filename.split('.')[0] == command:
break
else:
self.respond("Command '{}' not found.".format(command))
return
os.chdir(folder)
self.respond("Running '{}'...".format(command))
output = subprocess.check_output([filename],
stderr=subprocess.STDOUT,
universal_newlines=True)
if output.strip():
self.respond(output.strip())
def respond(self, msg):
'''Send a bluetooth response if connected.'''
print("> " + msg.replace('\n', '\n > '))
if not self.client_sock:
return
self.client_sock.send(msg + '\n')
def command_generator(self):
'''Establishes a client connection, yielding any commands given and
raising StopIteration upon close. '''
whitelist = self.settings.get('whitelist', [])
while True:
print("Waiting for connection on RFCOMM channel")
client_sock, client_info = self.server_sock.accept()
if whitelist and not client_info[0] in whitelist:
print("Denied connection from ", client_info)
client_sock.close()
continue
print("Accepted connection from ", client_info)
self.client_sock = client_sock
with contextlib.closing(client_sock):
while True:
try:
data = client_sock.recv(1024)
if not data:
raise StopIteration
yield data.decode('utf-8').strip()
except IOError:
traceback.print_exc()
except (KeyboardInterrupt, SystemExit, StopIteration):
raise
print("disconnected")
self.client_sock = None
def close(self):
'''Closes the server connection if open.'''
if self.server_sock:
self.server_sock.close()
class BluetoothInterface(BaseInterface):
"""The Bluetooth interface definition for communication with wireless client devices."""
__interface_name__ = 'bluetooth'
def __init__(self, controller):
"""
Initializes a bluetooth service that may be consumed by a remote client.
Arguments
---------
controller : controllers.base.BaseController
the parent controller that is instantiated this interface
"""
super(BluetoothInterface, self).__init__()
self.controller = controller
self.client_sock = None
self.client_info = None
self.rfcomm_channel = None
self.service_name = self.get_setting('service_name')
self.service_uuid = self.get_setting('service_uuid')
self.server_sock = None
self.thread = None
def connect(self):
"""Creates a new thread that listens for an incoming bluetooth RFCOMM connection."""
LOGGER.info('creating thread for bluetooth interface...')
self.thread = threading.Thread(
target=self.listen_for_rfcomm_connection)
self.thread.daemon = True
self.thread.start()
def listen_for_rfcomm_connection(self):
"""
Starts bluetooth interfaces
"""
# prepare bluetooth server
self.server_sock = BluetoothSocket(RFCOMM)
self.server_sock.bind(("", PORT_ANY))
self.server_sock.listen(1)
self.rfcomm_channel = self.server_sock.getsockname()[1]
# start listening for incoming connections
advertise_service(
self.server_sock,
self.service_name,
service_id=self.service_uuid,
service_classes=[self.service_uuid, SERIAL_PORT_CLASS],
profiles=[SERIAL_PORT_PROFILE])
LOGGER.info('waiting for connection on RFCOMM channel %d' %
self.rfcomm_channel)
# accept received connection
self.client_sock, self.client_info = self.server_sock.accept()
self.state = self.__states__.STATE_CONNECTED
LOGGER.info('accepted connection from %r', self.client_info)
# start listening for data
self.consume_bus()
def disconnect(self):
"""
Closes Bluetooth connection and resets handle
"""
LOGGER.info('destroying bluetooth interface...')
self.state = self.__states__.STATE_DISCONNECTING
if self.client_sock and hasattr(self.client_sock, 'close'):
self.client_sock.close()
self.client_sock = None
if self.server_sock and hasattr(self.server_sock, 'close'):
self.server_sock.close()
self.server_sock = None
# reset the bluetooth interface
self.thread = None
self.perform_hci0_reset()
self.state = self.__states__.STATE_READY
@staticmethod
def perform_hci0_reset():
"""Resets the bluetooth hci0 device via hciconfig command line interface."""
try:
LOGGER.info('performing hci0 down/up...')
subprocess.Popen('sudo hciconfig hci0 down',
shell=True).communicate()
subprocess.Popen('sudo hciconfig hci0 up',
shell=True).communicate()
LOGGER.info('hci0 down/up has completed')
except Exception as exception:
LOGGER.exception("Failed to restart hci0 - %r", exception)
def receive(self, data):
"""
Processes received data from Bluetooth socket
Arguments
---------
data : basestring
the data received from the bluetooth connection
"""
try:
packet = json.loads(data)
LOGGER.info('received packet via bluetooth: %r', packet['data'])
# invoke bound method (if set)
if self.receive_hook and hasattr(self.receive_hook, '__call__'):
self.receive_hook(packet['data'].decode('hex'))
except Exception as exception:
LOGGER.exception('error: %r', exception)
def send(self, data):
"""
Sends data via Bluetooth socket connection
Arguments
---------
data : basestring
the data to be sent via this interface
"""
if self.state != self.__states__.STATE_CONNECTED:
LOGGER.error('error: send() was called but state is not connected')
return False
try:
LOGGER.info('sending IBUSPacket(s)...')
packets = []
for packet in data:
packets.append(packet.as_serializable_dict())
# encapsulate ibus packets and send
data = {
"data": json.dumps(packets)
} # TODO : is an inner json.dumps necessary?
LOGGER.info(data)
self.client_sock.send(json.dumps(data))
except Exception as exception:
# socket was closed, graceful restart
LOGGER.exception('bt send: %r', exception.message)
self.reconnect()
def consume_bus(self):
"""
Start listening for incoming data
"""
try:
LOGGER.info('starting to listen for bluetooth data...')
read_buffer_length = self.get_setting('read_buffer_length', int)
while self.client_sock:
data = self.client_sock.recv(read_buffer_length)
if len(data) > 0:
self.receive(data)
except Exception as exception:
LOGGER.exception('android device was disconnected - %r', exception)
self.reconnect()
class Hal(object):
# class global, so that the front-end can cleanup on forced termination
# popen objects
cmds = []
# led blinker
led_blink_thread = None
led_blink_running = False
GYRO_MODES = {
'angle': 'GYRO-ANG',
'rate': 'GYRO-RATE',
}
LED_COLORS = {
'green': ev3dev.Leds.GREEN + ev3dev.Leds.GREEN,
'red': ev3dev.Leds.RED + ev3dev.Leds.RED,
'orange': ev3dev.Leds.ORANGE + ev3dev.Leds.ORANGE,
'black': ev3dev.Leds.BLACK + ev3dev.Leds.BLACK,
}
LED_ALL = ev3dev.Leds.LEFT + ev3dev.Leds.RIGHT
# usedSensors is unused, the code-generator for lab.openroberta > 1.4 wont
# pass it anymore
def __init__(self, brickConfiguration, usedSensors=None):
self.cfg = brickConfiguration
dir = os.path.dirname(__file__)
# char size: 6 x 12 -> num-chars: 29.666667 x 10.666667
self.font_s = ImageFont.load(os.path.join(dir, 'ter-u12n_unicode.pil'))
# char size: 10 x 18 -> num-chars: 17.800000 x 7.111111
# self.font_s = ImageFont.load(os.path.join(dir, 'ter-u18n_unicode.pil'))
self.lcd = ev3dev.Screen()
self.led = ev3dev.Leds
self.keys = ev3dev.Button()
self.sound = ev3dev.Sound
(self.font_w, self.font_h) = self.lcd.draw.textsize('X',
font=self.font_s)
# logger.info('char size: %d x %d -> num-chars: %f x %f',
# self.font_w, self.font_h, 178 / self.font_w, 128 / self.font_h)
self.timers = {}
self.sys_bus = None
self.bt_server = None
self.bt_connections = []
self.lang = 'de'
# factory methods
@staticmethod
# TODO(ensonic): remove 'side' param, server will stop using it >=3.3.0
# TODO(ensonic): 'regulated' is unused, it is passed to the motor-functions
# direcly, consider making all params after port 'kwargs'
def makeLargeMotor(port, regulated, direction, side=None):
try:
m = ev3dev.LargeMotor(port)
if direction == 'backward':
m.polarity = 'inversed'
else:
m.polarity = 'normal'
except (AttributeError, OSError):
logger.info('no large motor connected to port [%s]', port)
logger.exception("HW Config error")
m = None
return m
@staticmethod
def makeMediumMotor(port, regulated, direction, side=None):
try:
m = ev3dev.MediumMotor(port)
if direction == 'backward':
m.polarity = 'inversed'
else:
m.polarity = 'normal'
except (AttributeError, OSError):
logger.info('no medium motor connected to port [%s]', port)
logger.exception("HW Config error")
m = None
return m
@staticmethod
def makeOtherConsumer(port, regulated, direction, side=None):
try:
lp = ev3dev.LegoPort(port)
lp.mode = 'dc-motor'
# https://github.com/ev3dev/ev3dev-lang-python/issues/234
# some time is needed to set the permissions for the new attributes
time.sleep(0.5)
m = ev3dev.DcMotor(address=port)
except (AttributeError, OSError):
logger.info('no other consumer connected to port [%s]', port)
logger.exception("HW Config error")
m = None
return m
@staticmethod
def makeColorSensor(port):
try:
s = ev3dev.ColorSensor(port)
except (AttributeError, OSError):
logger.info('no color sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeGyroSensor(port):
try:
s = ev3dev.GyroSensor(port)
except (AttributeError, OSError):
logger.info('no gyro sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeI2cSensor(port):
try:
s = ev3dev.I2cSensor(port)
except (AttributeError, OSError):
logger.info('no i2c sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeInfraredSensor(port):
try:
s = ev3dev.InfraredSensor(port)
except (AttributeError, OSError):
logger.info('no infrared sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeLightSensor(port):
try:
p = ev3dev.LegoPort(port)
p.set_device = 'lego-nxt-light'
s = ev3dev.LightSensor(port)
except (AttributeError, OSError):
logger.info('no light sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeSoundSensor(port):
try:
p = ev3dev.LegoPort(port)
p.set_device = 'lego-nxt-sound'
s = ev3dev.SoundSensor(port)
except (AttributeError, OSError):
logger.info('no sound sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeTouchSensor(port):
try:
s = ev3dev.TouchSensor(port)
except (AttributeError, OSError):
logger.info('no touch sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeUltrasonicSensor(port):
try:
s = ev3dev.UltrasonicSensor(port)
except (AttributeError, OSError):
logger.info('no ultrasonic sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeCompassSensor(port):
try:
s = ev3dev.Sensor(address=port, driver_name='ht-nxt-compass')
except (AttributeError, OSError):
logger.info('no compass sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeIRSeekerSensor(port):
try:
s = ev3dev.Sensor(address=port, driver_name='ht-nxt-ir-seek-v2')
except (AttributeError, OSError):
logger.info('no ir seeker v2 sensor connected to port [%s]', port)
s = None
return s
@staticmethod
def makeHTColorSensorV2(port):
try:
s = ev3dev.Sensor(address=port, driver_name='ht-nxt-color-v2')
except (AttributeError, OSError):
logger.info('no hitechnic color sensor v2 connected to port [%s]',
port)
s = None
return s
# state
def resetState(self):
self.clearDisplay()
self.stopAllMotors()
self.resetAllOutputs()
self.resetLED()
logger.debug("terminate %d commands", len(Hal.cmds))
for cmd in Hal.cmds:
if cmd:
logger.debug("terminate command: %s", str(cmd))
cmd.terminate()
cmd.wait() # avoid zombie processes
Hal.cmds = []
# control
def waitFor(self, ms):
time.sleep(ms / 1000.0)
def busyWait(self):
'''Used as interrupptible busy wait.'''
time.sleep(0.0)
def waitCmd(self, cmd):
'''Wait for a command to finish.'''
Hal.cmds.append(cmd)
# we're not using cmd.wait() since that is not interruptable
while cmd.poll() is None:
self.busyWait()
Hal.cmds.remove(cmd)
# lcd
def drawText(self, msg, x, y, font=None):
font = font or self.font_s
self.lcd.draw.text((x * self.font_w, y * self.font_h), msg, font=font)
self.lcd.update()
def drawPicture(self, picture, x, y):
# logger.info('len(picture) = %d', len(picture))
size = (178, 128)
# One image is supposed to be 178*128/8 = 2848 bytes
# string data is in utf-16 format and padding with extra 0 bytes
data = bytes(picture, 'utf-16')[::2]
pixels = Image.frombytes('1', size, data, 'raw', '1;IR', 0, 1)
self.lcd.image.paste(pixels, (x, y))
self.lcd.update()
def clearDisplay(self):
self.lcd.clear()
self.lcd.update()
# led
def ledStopAnim(self):
if Hal.led_blink_running:
Hal.led_blink_running = False
Hal.led_blink_thread.join()
Hal.led_blink_thread = None
def ledOn(self, color, mode):
def ledAnim(anim):
while Hal.led_blink_running:
for step in anim:
self.led.set_color(Hal.LED_ALL, step[1])
time.sleep(step[0])
if not Hal.led_blink_running:
break
self.ledStopAnim()
# color: green, red, orange - LED.COLOR.{RED,GREEN,AMBER}
# mode: on, flash, double_flash
on = Hal.LED_COLORS[color]
off = Hal.LED_COLORS['black']
if mode == 'on':
self.led.set_color(Hal.LED_ALL, on)
elif mode == 'flash':
Hal.led_blink_thread = threading.Thread(target=ledAnim,
args=([(0.5, on),
(0.5, off)], ))
Hal.led_blink_running = True
Hal.led_blink_thread.start()
elif mode == 'double_flash':
Hal.led_blink_thread = threading.Thread(target=ledAnim,
args=([(0.15, on),
(0.15, off),
(0.15, on),
(0.55, off)], ))
Hal.led_blink_running = True
Hal.led_blink_thread.start()
def ledOff(self):
self.led.all_off()
self.ledStopAnim()
def resetLED(self):
self.ledOff()
# key
def isKeyPressed(self, key):
if key in ['any', '*']:
return self.keys.any()
else:
# remap some keys
key_aliases = {
'escape': 'backspace',
'back': 'backspace',
}
if key in key_aliases:
key = key_aliases[key]
return key in self.keys.buttons_pressed
def isKeyPressedAndReleased(self, key):
return False
# tones
def playTone(self, frequency, duration):
# this is already handled by the sound api (via beep cmd)
# frequency = frequency if frequency >= 100 else 0
self.waitCmd(self.sound.tone(frequency, duration))
def playFile(self, systemSound):
# systemSound is a enum for preset beeps:
# http://www.lejos.org/ev3/docs/lejos/hardware/Audio.html#systemSound-int-
# http://sf.net/p/lejos/ev3/code/ci/master/tree/ev3classes/src/lejos/remote/nxt/RemoteNXTAudio.java#l20
C2 = 523
if systemSound == 0:
self.playTone(600, 200)
elif systemSound == 1:
self.sound.tone([(600, 150, 50), (600, 150, 50)]).wait()
elif systemSound == 2: # C major arpeggio
self.sound.tone([(C2 * i / 4, 50, 50) for i in range(4, 7)]).wait()
elif systemSound == 3:
self.sound.tone([(C2 * i / 4, 50, 50)
for i in range(7, 4, -1)]).wait()
elif systemSound == 4:
self.playTone(100, 500)
def setVolume(self, volume):
self.sound.set_volume(volume)
def getVolume(self):
return self.sound.get_volume()
def setLanguage(self, lang):
# lang: 2digit ISO_639-1 code
self.lang = lang
def sayText(self, text, speed=30, pitch=50):
# a: amplitude, 0..200, def=100
# p: pitch, 0..99, def=50
# s: speed, 80..450, def=175
opts = '-a 200 -p %d -s %d -v %s' % (
int(clamp(pitch, 0, 100) * 0.99), # use range 0 - 99
int(clamp(speed, 0, 100) * 2.5 + 100), # use range 100 - 350
self.lang + "+f1") # female voice
self.waitCmd(self.sound.speak(text, espeak_opts=opts))
# actors
# http://www.ev3dev.org/docs/drivers/tacho-motor-class/
def scaleSpeed(self, m, speed_pct):
return int(speed_pct * m.max_speed / 100.0)
def rotateRegulatedMotor(self, port, speed_pct, mode, value):
# mode: degree, rotations, distance
m = self.cfg['actors'][port]
speed = self.scaleSpeed(m, clamp(speed_pct, -100, 100))
if mode == 'degree':
m.run_to_rel_pos(position_sp=value, speed_sp=speed)
while (m.state and 'stalled' not in m.state):
self.busyWait()
elif mode == 'rotations':
value *= m.count_per_rot
m.run_to_rel_pos(position_sp=int(value), speed_sp=speed)
while (m.state and 'stalled' not in m.state):
self.busyWait()
def rotateUnregulatedMotor(self, port, speed_pct, mode, value):
speed_pct = clamp(speed_pct, -100, 100)
m = self.cfg['actors'][port]
if mode == 'rotations':
value *= m.count_per_rot
if speed_pct >= 0:
value = m.position + value
m.run_direct(duty_cycle_sp=int(speed_pct))
while (m.position < value and 'stalled' not in m.state):
self.busyWait()
else:
value = m.position - value
m.run_direct(duty_cycle_sp=int(speed_pct))
while (m.position > value and 'stalled' not in m.state):
self.busyWait()
m.stop()
def turnOnRegulatedMotor(self, port, value):
m = self.cfg['actors'][port]
m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100)))
def turnOnUnregulatedMotor(self, port, value):
value = clamp(value, -100, 100)
self.cfg['actors'][port].run_direct(duty_cycle_sp=int(value))
def setRegulatedMotorSpeed(self, port, value):
m = self.cfg['actors'][port]
# https://github.com/rhempel/ev3dev-lang-python/issues/263
# m.speed_sp = self.scaleSpeed(m, clamp(value, -100, 100))
m.run_forever(speed_sp=self.scaleSpeed(m, clamp(value, -100, 100)))
def setUnregulatedMotorSpeed(self, port, value):
value = clamp(value, -100, 100)
self.cfg['actors'][port].duty_cycle_sp = int(value)
def getRegulatedMotorSpeed(self, port):
m = self.cfg['actors'][port]
return m.speed * 100.0 / m.max_speed
def getUnregulatedMotorSpeed(self, port):
return self.cfg['actors'][port].duty_cycle
def stopMotor(self, port, mode='float'):
# mode: float, nonfloat
# stop_actions: ['brake', 'coast', 'hold']
m = self.cfg['actors'][port]
if mode == 'float':
m.stop_action = 'coast'
elif mode == 'nonfloat':
m.stop_action = 'brake'
m.stop()
def stopMotors(self, left_port, right_port):
self.stopMotor(left_port)
self.stopMotor(right_port)
def stopAllMotors(self):
# [m for m in [Motor(port) for port in ['outA', 'outB', 'outC', 'outD']] if m.connected]
for file in glob.glob('/sys/class/tacho-motor/motor*/command'):
with open(file, 'w') as f:
f.write('stop')
for file in glob.glob('/sys/class/dc-motor/motor*/command'):
with open(file, 'w') as f:
f.write('stop')
def resetAllOutputs(self):
for port in (ev3dev.OUTPUT_A, ev3dev.OUTPUT_B, ev3dev.OUTPUT_C,
ev3dev.OUTPUT_D):
lp = ev3dev.LegoPort(port)
lp.mode = 'auto'
def regulatedDrive(self, left_port, right_port, reverse, direction,
speed_pct):
# direction: forward, backward
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
if direction == 'backward':
speed_pct = -speed_pct
ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct))
mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct))
def driveDistance(self, left_port, right_port, reverse, direction,
speed_pct, distance):
# direction: forward, backward
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
if direction == 'backward':
dc = -dc
# set all attributes
ml.stop_action = 'brake'
ml.position_sp = int(dc * ml.count_per_rot)
ml.speed_sp = self.scaleSpeed(ml, speed_pct)
mr.stop_action = 'brake'
mr.position_sp = int(dc * mr.count_per_rot)
mr.speed_sp = self.scaleSpeed(mr, speed_pct)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
# logger.debug("driving: %s, %s" % (ml.state, mr.state))
while (ml.state or mr.state):
self.busyWait()
def rotateDirectionRegulated(self, left_port, right_port, reverse,
direction, speed_pct):
# direction: left, right
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
if direction == 'left':
mr.run_forever(speed_sp=self.scaleSpeed(mr, speed_pct))
ml.run_forever(speed_sp=self.scaleSpeed(ml, -speed_pct))
else:
ml.run_forever(speed_sp=self.scaleSpeed(ml, speed_pct))
mr.run_forever(speed_sp=self.scaleSpeed(mr, -speed_pct))
def rotateDirectionAngle(self, left_port, right_port, reverse, direction,
speed_pct, angle):
# direction: left, right
# reverse: always false for now
speed_pct = clamp(speed_pct, -100, 100)
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
circ = math.pi * self.cfg['track-width']
distance = angle * circ / 360.0
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
logger.debug("doing %lf rotations" % dc)
# set all attributes
ml.stop_action = 'brake'
ml.speed_sp = self.scaleSpeed(ml, speed_pct)
mr.stop_action = 'brake'
mr.speed_sp = self.scaleSpeed(mr, speed_pct)
if direction == 'left':
mr.position_sp = int(dc * mr.count_per_rot)
ml.position_sp = int(-dc * ml.count_per_rot)
else:
ml.position_sp = int(dc * ml.count_per_rot)
mr.position_sp = int(-dc * mr.count_per_rot)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
logger.debug("turning: %s, %s" % (ml.state, mr.state))
while (ml.state or mr.state):
self.busyWait()
def driveInCurve(self,
direction,
left_port,
left_speed_pct,
right_port,
right_speed_pct,
distance=None):
# direction: foreward, backward
ml = self.cfg['actors'][left_port]
mr = self.cfg['actors'][right_port]
left_speed_pct = self.scaleSpeed(ml, clamp(left_speed_pct, -100, 100))
right_speed_pct = self.scaleSpeed(mr, clamp(right_speed_pct, -100,
100))
if distance:
left_dc = right_dc = 0.0
speed_pct = (left_speed_pct + right_speed_pct) / 2.0
if speed_pct:
circ = math.pi * self.cfg['wheel-diameter']
dc = distance / circ
left_dc = dc * left_speed_pct / speed_pct
right_dc = dc * right_speed_pct / speed_pct
# set all attributes
ml.stop_action = 'brake'
ml.speed_sp = int(left_speed_pct)
mr.stop_action = 'brake'
mr.speed_sp = int(right_speed_pct)
if direction == 'backward':
ml.position_sp = int(-left_dc * ml.count_per_rot)
mr.position_sp = int(-right_dc * mr.count_per_rot)
else:
ml.position_sp = int(left_dc * ml.count_per_rot)
mr.position_sp = int(right_dc * mr.count_per_rot)
# start motors
ml.run_to_rel_pos()
mr.run_to_rel_pos()
while ((ml.state and left_speed_pct)
or (mr.state and right_speed_pct)):
self.busyWait()
else:
if direction == 'backward':
ml.run_forever(speed_sp=int(-left_speed_pct))
mr.run_forever(speed_sp=int(-right_speed_pct))
else:
ml.run_forever(speed_sp=int(left_speed_pct))
mr.run_forever(speed_sp=int(right_speed_pct))
# sensors
def scaledValue(self, sensor):
return sensor.value() / float(10.0**sensor.decimals)
def scaledValues(self, sensor):
scale = float(10.0**sensor.decimals)
return tuple(
[sensor.value(i) / scale for i in range(sensor.num_values)])
# touch sensor
def isPressed(self, port):
return self.scaledValue(self.cfg['sensors'][port])
# ultrasonic sensor
def getUltraSonicSensorDistance(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'US-DIST-CM':
s.mode = 'US-DIST-CM'
return self.scaledValue(s)
def getUltraSonicSensorPresence(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'US-LISTEN':
s.mode = 'US-LISTEN'
return self.scaledValue(s) != 0.0
# gyro
# http://www.ev3dev.org/docs/sensors/lego-ev3-gyro-sensor/
def resetGyroSensor(self, port):
# change mode to reset for GYRO-ANG and GYRO-G&A
s = self.cfg['sensors'][port]
s.mode = 'GYRO-RATE'
s.mode = 'GYRO-ANG'
def getGyroSensorValue(self, port, mode):
s = self.cfg['sensors'][port]
if s.mode != Hal.GYRO_MODES[mode]:
s.mode = Hal.GYRO_MODES[mode]
return self.scaledValue(s)
# color
# http://www.ev3dev.org/docs/sensors/lego-ev3-color-sensor/
def getColorSensorAmbient(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'COL-AMBIENT':
s.mode = 'COL-AMBIENT'
return self.scaledValue(s)
def getColorSensorColour(self, port):
colors = [
'none', 'black', 'blue', 'green', 'yellow', 'red', 'white', 'brown'
]
s = self.cfg['sensors'][port]
if s.mode != 'COL-COLOR':
s.mode = 'COL-COLOR'
return colors[int(self.scaledValue(s))]
def getColorSensorRed(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'COL-REFLECT':
s.mode = 'COL-REFLECT'
return self.scaledValue(s)
def getColorSensorRgb(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'RGB-RAW':
s.mode = 'RGB-RAW'
return self.scaledValues(s)
# infrared
# http://www.ev3dev.org/docs/sensors/lego-ev3-infrared-sensor/
def getInfraredSensorSeek(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'IR-SEEK':
s.mode = 'IR-SEEK'
return self.scaledValues(s)
def getInfraredSensorDistance(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'IR-PROX':
s.mode = 'IR-PROX'
return self.scaledValue(s)
# timer
def getTimerValue(self, timer):
if timer in self.timers:
return int((time.time() - self.timers[timer]) * 1000.0)
else:
self.timers[timer] = time.time()
return 0
def resetTimer(self, timer):
self.timers[timer] = time.time()
# tacho-motor position
def resetMotorTacho(self, actorPort):
m = self.cfg['actors'][actorPort]
m.position = 0
def getMotorTachoValue(self, actorPort, mode):
m = self.cfg['actors'][actorPort]
tachoCount = m.position
if mode == 'degree':
return tachoCount * 360.0 / float(m.count_per_rot)
elif mode in ['rotation', 'distance']:
rotations = float(tachoCount) / float(m.count_per_rot)
if mode == 'rotation':
return rotations
else:
distance = round(math.pi * self.cfg['wheel-diameter'] *
rotations)
return distance
else:
raise ValueError('incorrect MotorTachoMode: %s' % mode)
def getSoundLevel(self, port):
# 100 for silent,
# 0 for loud
s = self.cfg['sensors'][port]
if s.mode != 'DB':
s.mode = 'DB'
return round(-self.scaledValue(s) + 100, 2) # map to 0 silent 100 loud
def getHiTecCompassSensorValue(self, port, mode):
s = self.cfg['sensors'][port]
if s.mode != 'COMPASS':
s.mode = 'COMPASS' # ev3dev currently only supports the compass mode
value = self.scaledValue(s)
if mode == 'angle':
return -(((value + 180) % 360) - 180
) # simulate the angle [-180, 180] mode from ev3lejos
else:
return value
def getHiTecIRSeekerSensorValue(self, port, mode):
s = self.cfg['sensors'][port]
if s.mode != mode:
s.mode = mode
value = self.scaledValue(s)
# remap from [1 - 9] default 0 to [120, -120] default NaN like ev3lejos
return float('nan') if value == 0 else (value - 5) * -30
def getHiTecColorSensorV2Colour(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'COLOR':
s.mode = 'COLOR'
value = s.value()
return self.mapHiTecColorIdToColor(int(value))
def mapHiTecColorIdToColor(self, id):
if id < 0 or id > 17:
return 'none'
colors = {
0: 'black',
1: 'red',
2: 'blue',
3: 'blue',
4: 'green',
5: 'yellow',
6: 'yellow',
7: 'red',
8: 'red',
9: 'red',
10: 'red',
11: 'white',
12: 'white',
13: 'white',
14: 'white',
15: 'white',
16: 'white',
17: 'white',
}
return colors[id]
def getHiTecColorSensorV2Ambient(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'PASSIVE':
s.mode = 'PASSIVE'
value = abs(s.value(0)) / 380
return min(value, 100)
def getHiTecColorSensorV2Light(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'NORM':
s.mode = 'NORM'
value = self.scaledValues(s)[3] / 2.55
return value
def getHiTecColorSensorV2Rgb(self, port):
s = self.cfg['sensors'][port]
if s.mode != 'NORM':
s.mode = 'NORM'
value = self.scaledValues(s)
value = list(value)
del value[0]
return value
def setHiTecColorSensorV2PowerMainsFrequency(self, port, frequency):
s = self.cfg['sensors'][port]
s.command = frequency
# communication
def _isTimeOut(self, e):
# BluetoothError seems to be an IOError, which is an OSError
# but all they do is: raise BluetoothError (str (e))
return str(e) == "timed out"
def establishConnectionTo(self, host):
# host can also be a name, resolving it is slow though and requires the
# device to be visible
if not bluetooth.is_valid_address(host):
nearby_devices = bluetooth.discover_devices()
for bdaddr in nearby_devices:
if host == bluetooth.lookup_name(bdaddr):
host = bdaddr
break
if bluetooth.is_valid_address(host):
con = BluetoothSocket(bluetooth.RFCOMM)
con.settimeout(0.5) # half second to make IO interruptible
while True:
try:
con.connect((host, 1)) # 0 is channel
self.bt_connections.append(con)
return len(self.bt_connections) - 1
except bluetooth.btcommon.BluetoothError as e:
if not self._isTimeOut(e):
logger.error("unhandled Bluetooth error: %s", repr(e))
break
else:
return -1
def waitForConnection(self):
# enable visibility
if not self.sys_bus:
self.sys_bus = dbus.SystemBus()
# do only once (since we turn off the timeout)
# alternatively set DiscoverableTimeout = 0 in /etc/bluetooth/main.conf
# and run hciconfig hci0 piscan, from robertalab initscript
hci0 = self.sys_bus.get_object('org.bluez', '/org/bluez/hci0')
props = dbus.Interface(hci0, 'org.freedesktop.DBus.Properties')
props.Set('org.bluez.Adapter1', 'DiscoverableTimeout', dbus.UInt32(0))
props.Set('org.bluez.Adapter1', 'Discoverable', True)
if not self.bt_server:
self.bt_server = BluetoothSocket(bluetooth.RFCOMM)
self.bt_server.settimeout(
0.5) # half second to make IO interruptible
self.bt_server.bind(("", bluetooth.PORT_ANY))
self.bt_server.listen(1)
while True:
try:
(con, info) = self.bt_server.accept()
con.settimeout(0.5) # half second to make IO interruptible
self.bt_connections.append(con)
return len(self.bt_connections) - 1
except bluetooth.btcommon.BluetoothError as e:
if not self._isTimeOut(e):
logger.error("unhandled Bluetooth error: %s", repr(e))
break
return -1
def readMessage(self, con_ix):
message = "NO MESSAGE"
if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]:
con = self.bt_connections[con_ix]
logger.debug('reading msg')
while True:
# TODO(ensonic): how much do we actually expect
# here is the lejos counter part
# https://github.com/OpenRoberta/robertalab-ev3lejos/blob/master/
# EV3Runtime/src/main/java/de/fhg/iais/roberta/runtime/ev3/BluetoothComImpl.java#L40..L59
try:
message = con.recv(128).decode('utf-8', errors='replace')
logger.debug('received msg [%s]' % message)
break
except bluetooth.btcommon.BluetoothError as e:
if not self._isTimeOut(e):
logger.error("unhandled Bluetooth error: %s", repr(e))
self.bt_connections[con_ix] = None
break
return message
def sendMessage(self, con_ix, message):
if con_ix < len(self.bt_connections) and self.bt_connections[con_ix]:
logger.debug('sending msg [%s]' % message)
con = self.bt_connections[con_ix]
while True:
try:
con.send(message)
logger.debug('sent msg')
break
except bluetooth.btcommon.BluetoothError as e:
if not self._isTimeOut(e):
logger.error("unhandled Bluetooth error: %s", repr(e))
self.bt_connections[con_ix] = None
break
class BluetoothConnection(object):
def __init__(self):
self.client_socket = None
self.server_socket = None
self.connected = False
def createServer(self, port=3):
self.server_socket = BluetoothSocket(RFCOMM)
self.server_socket.bind(("", port))
self.server_socket.listen(1)
print "Trying to connect.."
self.client_socket, _ = self.server_socket.accept()
print "Connected!"
self.connected = True
def createClient(self, address, port=3):
if address == "first":
address = self.searchDevices()[0]
self.client_socket = BluetoothSocket(RFCOMM)
print "Trying to connect.."
self.client_socket.connect((address, port))
print "Connected!"
self.connected = True
def searchDevices(self):
return discover_devices()
def receiveData(self, bufsize=1024):
if self.connected:
return self.client_socket.recv(bufsize)
else:
print "Not yet connected!"
def sendFile(self, filename):
f = open(filename, 'r')
data = f.readlines()
for line in data:
self.sendData(line)
f.close()
def receiveFile(self, filename, bufsize=4096):
data = self.receiveData(bufsize)
f = open(filename, 'w')
f.writelines(data)
f.flush()
f.close()
def sendData(self, data):
if self.connected:
self.client_socket.send(data)
def closeConnection(self):
if self.server_socket is not None:
self.server_socket.close()
self.client_socket.close()
self.connected = False
