Communication between motor and camera

Camera.py

@@ -121,44 +121,44 @@ 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']
             xmin = int(xmin * self.CAMERA_WIDTH)
             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']
             xmin = int(xmin * self.CAMERA_WIDTH)
             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
 

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()

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()