diff --git a/Camera.py b/Camera.py index 92847f3..24e181a 100644 --- a/Camera.py +++ b/Camera.py @@ -121,7 +121,7 @@ def annotate_objects(self, annotator, results, labels): annotator.text([xmin, ymin], '%s\n%.2f' % (labels[obj['class_id']], obj['score'])) - def detect_size(self, results, labels, obj_label): + def detect_sizes(self, results, labels): sizes = [] for obj in results: ymin, xmin, ymax, xmax = obj['bounding_box'] @@ -129,19 +129,19 @@ def detect_size(self, results, labels, obj_label): xmax = int(xmax * self.CAMERA_WIDTH) ymin = int(ymin * self.CAMERA_HEIGHT) ymax = int(ymax * self.CAMERA_HEIGHT) - if labels[obj['class_id']] == obj_label: - obj = {} - obj['height'] = xmax - xmin - obj['width'] = ymax - ymin - # 55mm width, 80mm height - obj['pixel_metric'] = (obj['width'] / 55 + obj['height'] / 80) / 2 - print("Pixel metrics: " + - str(round(obj['pixel_metric'], 1)) + "\n") - sizes.append(obj) + size_obj = {} + size_obj['name'] = labels[obj['class_id']] + size_obj['height'] = xmax - xmin + size_obj['width'] = ymax - ymin + # 55mm width, 80mm height + size_obj['pixel_metric'] = (size_obj['width'] / 55 + size_obj['height'] / 80) / 2 + print("Pixel metrics: " + + str(round(size_obj['pixel_metric'], 1)) + "\n") + sizes.append(size_obj) return sizes - def detect_distance(self, results, labels, obj_label): + def detect_distances(self, results, labels): distances = [] for obj in results: ymin, xmin, ymax, xmax = obj['bounding_box'] @@ -149,16 +149,16 @@ def detect_distance(self, results, labels, obj_label): xmax = int(xmax * self.CAMERA_WIDTH) ymin = int(ymin * self.CAMERA_HEIGHT) ymax = int(ymax * self.CAMERA_HEIGHT) - if labels[obj['class_id']] == obj_label: - obj = {} - obj['height'] = xmax - xmin - obj['width'] = ymax - ymin - # When pixel metric 2.1 distance will 155mm - obj['focal_distance'] = ( - (obj['width'] * 155) / 55 + obj['height'] * 155 / 80) / 2 - print("Focal distance: " + - str(round(obj['focal_distance'], 1)) + "\n") - distances.append(obj) + dist_obj = {} + dist_obj['name'] = labels[obj['class_id']] + dist_obj['height'] = xmax - xmin + dist_obj['width'] = ymax - ymin + # When pixel metric 2.1 distance will 155mm + dist_obj['focal_distance'] = ( + (dist_obj['width'] * 155) / 55 + dist_obj['height'] * 155 / 80) / 2 + print("Focal distance: " + + str(round(dist_obj['focal_distance'], 1)) + "\n") + distances.append(dist_obj) return distances def print_objects(self, results, labels): @@ -189,18 +189,18 @@ def execute_command(self): for interpreter in self.interpreters: image = image.resize((interpreter['shape'][1], interpreter['shape'][2]), Image.ANTIALIAS) - result = self.detect_objects(interpreter['interpreter'], image, 0.5) + results = self.detect_objects(interpreter['interpreter'], image, 0.5) # Annotate objects in terminal - self.print_objects(result, interpreter['labels']) + self.print_objects(results, interpreter['labels']) # Annotate object in view # self.annotate_objects(annotator, result, interpreter['labels']) - # Detect size and distance TODO: improve with contanstant object - size = self.detect_size( - result, interpreter['labels'], interpreter['name']) - distance = self.detect_distance( - result, interpreter['labels'], interpreter['name']) + # Detect size and distance + # TODO: improve with physical object with 1cm length + sizes = self.detect_sizes(results, interpreter['labels']) + distances = self.detect_distances(results, interpreter['labels']) if bool(interpreter.get('function')): - interpreter['function'](result, interpreter['labels'], size, distance) + interpreter['function']( + results, interpreter['labels'], sizes, distances, interpreter['name']) elapsed_ms = (time.monotonic() - start_time) * 1000 diff --git a/Excavator.py b/Excavator.py index f928c84..af57fc9 100644 --- a/Excavator.py +++ b/Excavator.py @@ -57,6 +57,14 @@ def backward_right_chain(self, speed=100): clockwise=False, speed=speed) + def move_forward(self, speed=100): + self.forward_left_chain(speed) + self.forward_right_chain(speed) + + def move_backward(self, speed=100): + self.backward_left_chain(speed) + self.backward_right_chain(speed) + def turn_left_body(self, speed=100): self.motors_memo.append(self.BODY_MOTOR) self.motors.run_dc_motor(self.BODY_MOTOR, clockwise=True, speed=speed) @@ -77,6 +85,11 @@ def move_down_shovel(self, speed=100): clockwise=False, speed=speed) + def stop_all_motors(self): + self.motors.stop_dc_motors([self.LEFT_CHAIN_MOTOR, self.RIGHT_CHAIN_MOTOR, self.BODY_MOTOR, self.SHOVEL_MOTOR]) + self.motors_memo = [] + time.sleep(1) + def test_move(self): self.forward_left_chain() self.forward_right_chain() diff --git a/camera_node.py b/camera_node.py new file mode 100644 index 0000000..c309f78 --- /dev/null +++ b/camera_node.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 + +import sys +import socket +import selectors +import traceback +from Camera import Camera + +from client_message import Message + +class CameraNode: + def __init__(self) -> None: + self.sel = selectors.DefaultSelector() + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + try: + self.sel.close() + except RuntimeWarning: + return True + + def create_request(self, action, value, encode="utf-8"): + if encode == "bin": + return dict( + type="binary/custom-client-binary-type", + encoding="binary", + content=bytes(action + value, encoding="utf-8"), + ) + else: + return dict( + type="text/json", + encoding="utf-8", + content=dict(action=action, value=value), + ) + + def start_connection(self): + self.addr = ('127.0.0.1', 65432) + print("starting connection to", self.addr) + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.setblocking(False) + self.sock.connect_ex(self.addr) + + def send_instruction(self, request): + events = selectors.EVENT_READ | selectors.EVENT_WRITE + message = Message(self.sel, self.sock, self.addr, request) + self.sel.register(self.sock, events, data=message) + + try: + while True: + events = self.sel.select(timeout=1) + for key, mask in events: + message = key.data + try: + message.process_events(mask) + except Exception: + print( + "main: error: exception for", + f"{message.addr}:\n{traceback.format_exc()}", + ) + message.close() + # Check for a socket being monitored to continue. + if not self.sel.get_map(): + break + except KeyboardInterrupt: + print("caught keyboard interrupt, exiting") + finally: + self.sel.close() + + def send_request(self, action, value): + request = self.create_request(action, value) + self.start_connection() + self.send_instruction(request) + +def find_object(results, labels, sizes, distances, obj_name): + cnode = CameraNode() + score = 0 + + for obj in results: + if labels[obj['class_id']] == obj_name: + score = obj['score'] + + obj_size = next((size for size in sizes if size["name"] == obj_name), None) + obj_dist = next((dist for dist in distances if dist["name"] == obj_name), None) + + while score < 0.5: + cnode.send_request("left", "4") + cnode.send_request("right", "4") + + cnode.send_request("stop", "all") + + while obj_dist > 100: + cnode.send_request("forward", "1") + + cnode.send_request("stop", "all") + +tl_models = [ + { + 'name': 'shovel', + 'model_path': './trained_model/shovel_model/model.tflite', + 'label_path': './trained_model/shovel_model/model-dict.txt', + 'function': None + }, + { + 'name': 'apple', + 'model_path': './trained_model/object/detect.tflite', + 'label_path': './trained_model/object/coco_labels.txt', + 'function': find_object + } +] + +camera = Camera(tl_models) +camera.execute_command() diff --git a/client_message.py b/client_message.py new file mode 100644 index 0000000..0cc2ed8 --- /dev/null +++ b/client_message.py @@ -0,0 +1,211 @@ +import sys +import selectors +import json +import io +import struct + +class Message: + def __init__(self, selector, sock, addr, request): + self.selector = selector + self.sock = sock + self.addr = addr + self._recv_buffer = b"" + self._send_buffer = b"" + self.request = request + self._request_queued = False + self._jsonheader_len = None + self.jsonheader = None + self.response = None + + def _set_selector_events_mask(self, mode): + """Set selector to listen for events: mode is 'r', 'w', or 'rw'.""" + if mode == "r": + events = selectors.EVENT_READ + elif mode == "w": + events = selectors.EVENT_WRITE + elif mode == "rw": + events = selectors.EVENT_READ | selectors.EVENT_WRITE + else: + raise ValueError(f"Invalid events mask mode {repr(mode)}.") + self.selector.modify(self.sock, events, data=self) + + def _read(self): + try: + # Should be ready to read + data = self.sock.recv(4096) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + if data: + self._recv_buffer += data + else: + raise RuntimeError("Peer closed.") + + def _write(self): + if self._send_buffer: + print("sending", repr(self._send_buffer), "to", self.addr) + try: + # Should be ready to write + sent = self.sock.send(self._send_buffer) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + self._send_buffer = self._send_buffer[sent:] + + def _json_encode(self, obj, encoding): + return json.dumps(obj, ensure_ascii=False).encode(encoding) + + def _json_decode(self, json_bytes, encoding): + tiow = io.TextIOWrapper( + io.BytesIO(json_bytes), encoding=encoding, newline="" + ) + obj = json.load(tiow) + tiow.close() + return obj + + def _create_message( + self, *, content_bytes, content_type, content_encoding + ): + jsonheader = { + "byteorder": sys.byteorder, + "content-type": content_type, + "content-encoding": content_encoding, + "content-length": len(content_bytes), + } + jsonheader_bytes = self._json_encode(jsonheader, "utf-8") + message_hdr = struct.pack(">H", len(jsonheader_bytes)) + message = message_hdr + jsonheader_bytes + content_bytes + return message + + def _process_response_json_content(self): + content = self.response + result = content.get("result") + print(f"got result: {result}") + + def _process_response_binary_content(self): + content = self.response + print(f"got response: {repr(content)}") + + def process_events(self, mask): + if mask & selectors.EVENT_READ: + self.read() + if mask & selectors.EVENT_WRITE: + self.write() + + def read(self): + self._read() + + if self._jsonheader_len is None: + self.process_protoheader() + + if self._jsonheader_len is not None: + if self.jsonheader is None: + self.process_jsonheader() + + if self.jsonheader: + if self.response is None: + self.process_response() + + def write(self): + if not self._request_queued: + self.queue_request() + + self._write() + + if self._request_queued: + if not self._send_buffer: + # Set selector to listen for read events, we're done writing. + self._set_selector_events_mask("r") + + def close(self): + print("closing connection to", self.addr) + try: + self.selector.unregister(self.sock) + except Exception as e: + print( + "error: selector.unregister() exception for", + f"{self.addr}: {repr(e)}", + ) + + try: + self.sock.close() + except OSError as e: + print( + "error: socket.close() exception for", + f"{self.addr}: {repr(e)}", + ) + finally: + # Delete reference to socket object for garbage collection + self.sock = None + + def set_request(self, request): + self.request = request + self._request_queued = False + + def queue_request(self): + content = self.request["content"] + content_type = self.request["type"] + content_encoding = self.request["encoding"] + if content_type == "text/json": + req = { + "content_bytes": self._json_encode(content, content_encoding), + "content_type": content_type, + "content_encoding": content_encoding, + } + else: + req = { + "content_bytes": content, + "content_type": content_type, + "content_encoding": content_encoding, + } + message = self._create_message(**req) + self._send_buffer += message + self._request_queued = True + + def process_protoheader(self): + hdrlen = 2 + if len(self._recv_buffer) >= hdrlen: + self._jsonheader_len = struct.unpack( + ">H", self._recv_buffer[:hdrlen] + )[0] + self._recv_buffer = self._recv_buffer[hdrlen:] + + def process_jsonheader(self): + hdrlen = self._jsonheader_len + if len(self._recv_buffer) >= hdrlen: + self.jsonheader = self._json_decode( + self._recv_buffer[:hdrlen], "utf-8" + ) + self._recv_buffer = self._recv_buffer[hdrlen:] + for reqhdr in ( + "byteorder", + "content-length", + "content-type", + "content-encoding", + ): + if reqhdr not in self.jsonheader: + raise ValueError(f'Missing required header "{reqhdr}".') + + def process_response(self): + content_len = self.jsonheader["content-length"] + if not len(self._recv_buffer) >= content_len: + return + data = self._recv_buffer[:content_len] + self._recv_buffer = self._recv_buffer[content_len:] + if self.jsonheader["content-type"] == "text/json": + encoding = self.jsonheader["content-encoding"] + self.response = self._json_decode(data, encoding) + print("received response", repr(self.response), "from", self.addr) + self._process_response_json_content() + else: + # Binary or unknown content-type + self.response = data + print( + f'received {self.jsonheader["content-type"]} response from', + self.addr, + ) + self._process_response_binary_content() + # Close when response has been processed + # self.close() diff --git a/motor_node.py b/motor_node.py new file mode 100644 index 0000000..76a9abf --- /dev/null +++ b/motor_node.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +import sys +import socket +import selectors +import traceback + +from server_message import Message +from Excavator import Excavator + +class MotorNode: + def __init__(self) -> None: + self.sel = selectors.DefaultSelector() + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_val, exc_tb): + try: + self.sel.close() + except RuntimeWarning: + return True + + def accept_wrapper(self, sock): + conn, addr = sock.accept() # Should be ready to read + print("accepted connection from", addr) + conn.setblocking(False) + message = Message(self.sel, conn, addr, instructions) + self.sel.register(conn, selectors.EVENT_READ, data=message) + + def init_listener(self): + addr = ('127.0.0.1', 65432) + lsock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + # Avoid bind() exception: OSError: [Errno 48] Address already in use + lsock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + lsock.bind(addr) + lsock.listen() + print("listening on", addr) + lsock.setblocking(False) + self.sel.register(lsock, selectors.EVENT_READ, data=None) + + def follow_instructions(self): + try: + while True: + events = self.sel.select(timeout=None) + for key, mask in events: + if key.data is None: + print("Accept wrapper is executed") + self.accept_wrapper(key.fileobj) + else: + message = key.data + try: + message.process_events(mask) + except Exception: + print( + "main: error: exception for", + f"{message.addr}:\n{traceback.format_exc()}", + ) + message.close() + except KeyboardInterrupt: + print("caught keyboard interrupt, exiting") + finally: + self.sel.close() + + +excavator = Excavator() +instructions = { + "forward": {'cmd': excavator.move_forward, 'fire': excavator.execute}, + "backward": {'cmd': excavator.move_forward, 'fire': excavator.execute}, + "left": {'cmd': excavator.forward_left_chain, 'fire': excavator.execute}, + "right": {'cmd': excavator.forward_right_chain, 'fire': excavator.execute}, + "shovel-left": {'cmd': excavator.turn_left_body, 'fire': excavator.execute}, + "shovel-right": {'cmd': excavator.turn_right_body, 'fire': excavator.execute}, + "shovel-up": {'cmd': excavator.move_up_shovel, 'fire': excavator.execute}, + "shovel-down": {'cmd': excavator.move_down_shovel, 'fire': excavator.execute}, + "stop": {'cmd': excavator.stop_all_motors} +} + +motor_node = MotorNode() +motor_node.init_listener() +motor_node.follow_instructions() diff --git a/server_message.py b/server_message.py new file mode 100644 index 0000000..f4604ba --- /dev/null +++ b/server_message.py @@ -0,0 +1,213 @@ +import sys +import selectors +import json +import io +import struct + +class Message: + def __init__(self, selector, sock, addr, instructions): + self.selector = selector + self.sock = sock + self.addr = addr + self._recv_buffer = b"" + self._send_buffer = b"" + self._jsonheader_len = None + self.jsonheader = None + self.request = None + self.response_created = False + self.instructions = instructions + + def _set_selector_events_mask(self, mode): + """Set selector to listen for events: mode is 'r', 'w', or 'rw'.""" + if mode == "r": + events = selectors.EVENT_READ + elif mode == "w": + events = selectors.EVENT_WRITE + elif mode == "rw": + events = selectors.EVENT_READ | selectors.EVENT_WRITE + else: + raise ValueError(f"Invalid events mask mode {repr(mode)}.") + self.selector.modify(self.sock, events, data=self) + + def _read(self): + try: + # Should be ready to read + data = self.sock.recv(4096) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + if data: + self._recv_buffer += data + else: + raise RuntimeError("Peer closed.") + + def _write(self): + if self._send_buffer: + print("sending", repr(self._send_buffer), "to", self.addr) + try: + # Should be ready to write + sent = self.sock.send(self._send_buffer) + except BlockingIOError: + # Resource temporarily unavailable (errno EWOULDBLOCK) + pass + else: + self._send_buffer = self._send_buffer[sent:] + # Close when the buffer is drained. The response has been sent. + if sent and not self._send_buffer: + self.close() + + def _json_encode(self, obj, encoding): + return json.dumps(obj, ensure_ascii=False).encode(encoding) + + def _json_decode(self, json_bytes, encoding): + tiow = io.TextIOWrapper( + io.BytesIO(json_bytes), encoding=encoding, newline="" + ) + obj = json.load(tiow) + tiow.close() + return obj + + def _create_message( + self, *, content_bytes, content_type, content_encoding + ): + jsonheader = { + "byteorder": sys.byteorder, + "content-type": content_type, + "content-encoding": content_encoding, + "content-length": len(content_bytes), + } + jsonheader_bytes = self._json_encode(jsonheader, "utf-8") + message_hdr = struct.pack(">H", len(jsonheader_bytes)) + message = message_hdr + jsonheader_bytes + content_bytes + return message + + def _create_response_json_content(self): + action = self.request.get("action") + query = self.request.get("value") + if action == "hello": + content = {"result": "hello camera"} + elif bool(self.instructions.get(action)): + self.instructions.get(action)['cmd']() + self.instructions.get(action)['fire'](int(query)) + content = {"result": action} + else: + content = {"result": f'Error: invalid action "{action}".'} + content_encoding = "utf-8" + response = { + "content_bytes": self._json_encode(content, content_encoding), + "content_type": "text/json", + "content_encoding": content_encoding, + } + return response + + def _create_response_binary_content(self): + response = { + "content_bytes": b"First 10 bytes of request: " + + self.request[:10], + "content_type": "binary/custom-server-binary-type", + "content_encoding": "binary", + } + return response + + def process_events(self, mask): + if mask & selectors.EVENT_READ: + self.read() + if mask & selectors.EVENT_WRITE: + self.write() + + def read(self): + self._read() + + if self._jsonheader_len is None: + self.process_protoheader() + + if self._jsonheader_len is not None: + if self.jsonheader is None: + self.process_jsonheader() + + if self.jsonheader: + if self.request is None: + self.process_request() + + def write(self): + if self.request: + if not self.response_created: + self.create_response() + + self._write() + + def close(self): + print("closing connection to", self.addr) + try: + self.selector.unregister(self.sock) + except Exception as e: + print( + "error: selector.unregister() exception for", + f"{self.addr}: {repr(e)}", + ) + + try: + self.sock.close() + except OSError as e: + print( + "error: socket.close() exception for", + f"{self.addr}: {repr(e)}", + ) + finally: + # Delete reference to socket object for garbage collection + self.sock = None + + def process_protoheader(self): + hdrlen = 2 + if len(self._recv_buffer) >= hdrlen: + self._jsonheader_len = struct.unpack( + ">H", self._recv_buffer[:hdrlen] + )[0] + self._recv_buffer = self._recv_buffer[hdrlen:] + + def process_jsonheader(self): + hdrlen = self._jsonheader_len + if len(self._recv_buffer) >= hdrlen: + self.jsonheader = self._json_decode( + self._recv_buffer[:hdrlen], "utf-8" + ) + self._recv_buffer = self._recv_buffer[hdrlen:] + for reqhdr in ( + "byteorder", + "content-length", + "content-type", + "content-encoding", + ): + if reqhdr not in self.jsonheader: + raise ValueError(f'Missing required header "{reqhdr}".') + + def process_request(self): + content_len = self.jsonheader["content-length"] + if not len(self._recv_buffer) >= content_len: + return + data = self._recv_buffer[:content_len] + self._recv_buffer = self._recv_buffer[content_len:] + if self.jsonheader["content-type"] == "text/json": + encoding = self.jsonheader["content-encoding"] + self.request = self._json_decode(data, encoding) + print("received request", repr(self.request), "from", self.addr) + else: + # Binary or unknown content-type + self.request = data + print( + f'received {self.jsonheader["content-type"]} request from', + self.addr, + ) + # Set selector to listen for write events, we're done reading. + self._set_selector_events_mask("w") + + def create_response(self): + if self.jsonheader["content-type"] == "text/json": + response = self._create_response_json_content() + else: + # Binary or unknown content-type + response = self._create_response_binary_content() + message = self._create_message(**response) + self.response_created = True + self._send_buffer += message diff --git a/detect_picamera.py b/test_picamera.py similarity index 100% rename from detect_picamera.py rename to test_picamera.py