diff --git a/animations/curious.json b/animations/curious.json new file mode 100644 index 00000000..b8d6dbd1 --- /dev/null +++ b/animations/curious.json @@ -0,0 +1,12 @@ +[ + {"servo:tilt:mv": 10}, + {"sleep": 0.5}, + {"servo:pan:mv": 15}, + {"sleep": 0.5}, + {"servo:pan:mv": -15}, + {"sleep": 0.5}, + {"servo:tilt:mv": -10}, + {"sleep": 0.5}, + {"servo:pan:mv": 0}, + {"servo:tilt:mv": 0} +] \ No newline at end of file diff --git a/animations/excitement.json b/animations/excitement.json new file mode 100644 index 00000000..3d3e9a00 --- /dev/null +++ b/animations/excitement.json @@ -0,0 +1,17 @@ +[ + {"servo:pan:mv": 30}, + {"servo:tilt:mv": 20}, + {"sleep": 0.3}, + {"servo:pan:mv": -30}, + {"servo:tilt:mv": -20}, + {"sleep": 0.3}, + {"servo:pan:mv": 40}, + {"servo:tilt:mv": 30}, + {"sleep": 0.3}, + {"servo:pan:mv": -40}, + {"servo:tilt:mv": -30}, + {"sleep": 0.3}, + {"servo:pan:mv": 0}, + {"servo:tilt:mv": 0}, + {"sleep": 1} +] \ No newline at end of file diff --git a/animations/excitement2.json b/animations/excitement2.json new file mode 100644 index 00000000..5afce591 --- /dev/null +++ b/animations/excitement2.json @@ -0,0 +1,14 @@ +[ + {"servo:pan:mv": 30}, + {"servo:tilt:mv": 20}, + {"led/color": "blue"}, + {"sleep": 0.3}, + {"servo:pan:mv": -30}, + {"servo:tilt:mv": -20}, + {"led/color": "red"}, + {"sleep": 0.3}, + {"servo:pan:mv": 0}, + {"servo:tilt:mv": 0}, + {"led/color": "green"}, + {"sleep": 1} +] \ No newline at end of file diff --git a/animations/level_neck.json b/animations/level_neck.json new file mode 100644 index 00000000..f2404c3d --- /dev/null +++ b/animations/level_neck.json @@ -0,0 +1,4 @@ +[ + {"servo:tilt:mvabs": 50}, + {"sleep": 1} +] \ No newline at end of file diff --git a/animations/sadness.json b/animations/sadness.json new file mode 100644 index 00000000..669be916 --- /dev/null +++ b/animations/sadness.json @@ -0,0 +1,10 @@ +[ + {"servo:tilt:mv": -20}, + {"sleep": 1}, + {"servo:pan:mv": -10}, + {"sleep": 1}, + {"servo:pan:mv": 10}, + {"sleep": 1}, + {"servo:pan:mv": 0}, + {"servo:tilt:mv": 0} +] \ No newline at end of file diff --git a/animations/surprise.json b/animations/surprise.json new file mode 100644 index 00000000..97a05dc2 --- /dev/null +++ b/animations/surprise.json @@ -0,0 +1,11 @@ +[ + {"piservo/move": 40}, + {"servo:tilt:mv": 30}, + {"sleep": 0.2}, + {"servo:pan:mv": 20}, + {"sleep": 0.2}, + {"servo:pan:mv": -20}, + {"sleep": 0.2}, + {"servo:tilt:mv": 0}, + {"sleep": 0.5} +] \ No newline at end of file diff --git a/arduino_sketch/Config.h b/arduino_sketch/Config.h index 1c8bb900..12c395c8 100644 --- a/arduino_sketch/Config.h +++ b/arduino_sketch/Config.h @@ -66,7 +66,7 @@ int PosSleep[SERVO_COUNT] = {40, 60, 95, 140, 120, 85, PosMax[7], 90, 180, 0}; //0, 3 = HIP int PosStart[SERVO_COUNT] = {60, 0, 165, 120, 180, 30, 90, 90, 180, 0}; -int PosBackpack[SERVO_COUNT] = {30, 5, 90, 130, 120, 160, 90, 90, 180, 0}; // Position legs to support when mounted to backpack +int PosBackpack[SERVO_COUNT] = {30, 5, 130, 130, 120, 160, 90, 90, 180, 0}; // Position legs to support when mounted to backpack int PosStraight[SERVO_COUNT] = {45, 90, 165, 135, 90, 20, 90, 90, 180, 0}; // straighten legs and point feet to fit in backpack upright //int PosRest[SERVO_COUNT] = {S1_REST, S2_REST, S3_REST, S4_REST, S5_REST, S6_REST, S7_REST, S8_REST, S9_REST}; @@ -79,6 +79,8 @@ int PosStand[SERVO_COUNT] = {45, 70, 80, 135, 110, 100, NOVAL, NOVAL, 180, 0}; int PosLookLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 180, 180, 0}; int PosLookRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 0, 180, 0}; int PosLookRandom[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, -1, 180, 0}; // Made random by calling the function moveRandom() if the value is -1 +int PosLookRandomRight[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, 30, 180, 0}; +int PosLookRandomLeft[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, -1, 150, 180, 0}; int PosLookUp[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 60, 90, 180, 0}; int PosLookDown[SERVO_COUNT] = {NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, NOVAL, 120, 90, 180, 0}; diff --git a/config/vision.yml b/config/vision_imx500.yml similarity index 82% rename from config/vision.yml rename to config/vision_imx500.yml index 63800fa9..cfb09b75 100644 --- a/config/vision.yml +++ b/config/vision_imx500.yml @@ -1,5 +1,5 @@ -vision: - enabled: true +vision_imx500: + enabled: false path: 'modules.vision.imx500.vision.Vision' config: preview: false diff --git a/config/vision_opencv.yml b/config/vision_opencv.yml new file mode 100644 index 00000000..0377dc07 --- /dev/null +++ b/config/vision_opencv.yml @@ -0,0 +1,5 @@ +vision_opencv: + enabled: true + path: 'modules.vision.opencv.vision.Vision' + config: + preview: false diff --git a/docs/NeoPx.md b/docs/NeoPx.md new file mode 100644 index 00000000..e69de29b diff --git a/modules/archived/opencv/vision.py b/modules/archived/opencv/vision.py index 6fd9f781..b52ef010 100644 --- a/modules/archived/opencv/vision.py +++ b/modules/archived/opencv/vision.py @@ -2,24 +2,31 @@ import cv2 from imutils.video import FPS # for FSP only -try: - from modules.opencv.faces import Faces -except ModuleNotFoundError as e: - # Local execution - from faces import Faces - import os - from video_stream import VideoStream +from modules.vision.opencv.faces import Faces +from modules.vision.opencv.video_stream import VideoStream +from modules.base_module import BaseModule -from pubsub import pub -class Vision: +def list_available_cameras(max_index=10): + available = [] + for idx in range(max_index): + cap = cv2.VideoCapture(idx) + if cap.isOpened(): + available.append(idx) + cap.release() + return available + + +class Vision(BaseModule): MODE_MOTION = 0 MODE_FACES = 1 - def __init__(self, video, **kwargs): + def __init__(self, **kwargs): self.mode = kwargs.get('mode', Vision.MODE_MOTION) - self.path = kwargs.get('path', '/') + self.path = kwargs.get('path', '.') + # Make sure to use the correct camera index for your Pi camera + # Usually, index 0 is correct for the standard Pi camera self.index = kwargs.get('index', 0) self.static_back = None self.dimensions = kwargs.get('resolution', (640, 480)) @@ -28,29 +35,41 @@ def __init__(self, video, **kwargs): self.flip = kwargs.get('flip', False) self.rotate = kwargs.get('rotate', False) - self.video = video + self.video = VideoStream( + resolution=self.dimensions, + index=self.index + ) self.lines = [] self.current_match = False self.last_match = datetime.now() # @todo improve - pub.subscribe(self.exit, "exit") # start the FPS counter self.fps = FPS().start() if self.mode == Vision.MODE_FACES: - self.cascade_path = self.path + "/modules/opencv/haarcascade_frontalface_default.xml" + self.cascade_path = self.path + "/modules/vision/opencv/haarcascade_frontalface_default.xml" + print(f"[Vision] Loading cascade from {self.cascade_path}") self.cascade = cv2.CascadeClassifier(self.cascade_path) self.faces = Faces(detector=self.cascade, path=self.path) + print("[Vision] Scanning for available cameras...") + available_cams = list_available_cameras(10) + print(f"[Vision] Available camera indexes: {available_cams}") + + self.video.start() self.running = True + def setup_messaging(self): + """Subscribe to necessary topics.""" + self.subscribe('system/loop', self.detect) + def exit(self): self.running = False self.video.stop() # Destroying all the windows cv2.destroyAllWindows() self.fps.stop() - pub.sendMessage("log", msg="[Vision] Approx. FPS: {:.2f}".format(self.fps.fps())) + self.log("[Vision] Approx. FPS: {:.2f}".format(self.fps.fps()), 'debug') def reset(self): self.static_back = None @@ -58,16 +77,16 @@ def reset(self): def detect(self): if not self.running: return - # if not self.video.stream.isOpened(): - # raise Exception('Unable to load camera') + if not self.video.stream.isOpened(): + raise Exception('Unable to load camera') # update the FPS counter self.fps.update() matches = [] frame = self.video.read() - if frame is None: - return + if frame is None or frame is False: + raise Exception("[Vision] No frame captured, stopping detection") if self.flip is True: frame = cv2.flip(frame, 0) @@ -88,8 +107,8 @@ def detect(self): if len(matches) < 1: if self.current_match: self.current_match = False - pub.sendMessage('vision:nomatch') if self.preview is False: + print("[Vision] No faces detected") return matches names = [] @@ -142,6 +161,8 @@ def detect(self): if len(matches) > 0: self.current_match = True self.last_match = datetime.now() + + print(f"[Vision] Detected {len(matches)} matches") return matches diff --git a/modules/display/tft_display_eye.py b/modules/display/tft_display_eye.py new file mode 100644 index 00000000..e69de29b diff --git a/modules/personality.py b/modules/personality.py index a288fb8d..7000e3c7 100644 --- a/modules/personality.py +++ b/modules/personality.py @@ -63,8 +63,10 @@ def loop(self): self.random_animation() def random_animation(self): + self.publish('animate', action='level_neck') animations = [ 'head_shake', + 'head_nod', 'head_left', 'head_right', # 'look_down', diff --git a/modules/vision/imx500/tracking.py b/modules/vision/imx500/tracking.py index e5e26d6b..fa34e9d6 100644 --- a/modules/vision/imx500/tracking.py +++ b/modules/vision/imx500/tracking.py @@ -64,8 +64,6 @@ def handle(self, matches): """Handle new detections by processing in an asynchronous thread.""" if not self.active or self.moving: return - # print("Handling matches") - # print(matches) asyncio.run(self.process_matches(matches)) @staticmethod diff --git a/modules/vision/opencv/vision.py b/modules/vision/opencv/vision.py new file mode 100644 index 00000000..6bf43946 --- /dev/null +++ b/modules/vision/opencv/vision.py @@ -0,0 +1,170 @@ +import argparse +import sys +import cv2 +import numpy as np +from functools import lru_cache + +from modules.base_module import BaseModule + +# Add picamera2 imports +from picamera2 import Picamera2, Preview + +class Detection: + def __init__(self, box, conf, selfref): + self.box = box # (x, y, w, h) + self.conf = conf + self.piCamImx500 = selfref + # Calculate distances from center (assume 640x480 frame) + x, y, w, h = self.box + detection_center_x = x + w // 2 + detection_center_y = y + h // 2 + screen_center_x = 640 // 2 + screen_center_y = 480 // 2 + self.distance_x = int(detection_center_x - screen_center_x) + self.distance_y = int(detection_center_y - screen_center_y) + + def display(self): + label = f"face ({self.conf:.2f}): {self.box}" + print(label) + print("") + + def json_out(self): + return { + 'category': 'person', + 'confidence': str(self.conf), + 'bbox': list(map(int, self.box)), + 'distance_x': self.distance_x, + 'distance_y': self.distance_y, + } + +class Vision(BaseModule): + def __init__(self, **kwargs): + """ + Simple face detection using OpenCV Haar cascades. + Dependencies: opencv-python, numpy, modules.base_module, picamera2 + """ + self.last_detections = [] + self.last_results = [] + self.args = Vision.get_args() + # Use OpenCV's built-in Haar cascade for face detection + self.face_cascade = cv2.CascadeClassifier( + cv2.data.haarcascades + "haarcascade_frontalface_default.xml" + ) + # Use PiCamera2 instead of VideoCapture + cam_index = getattr(self.args, "camera", 0) + # Select camera if multiple are present + self.picam2 = Picamera2(camera_num=cam_index) + self.picam2.configure(self.picam2.create_preview_configuration(main={"format": 'RGB888', "size": (640, 480)})) + self.picam2.start() + # Stabilization state + self.previous_frame = None + self.stable_frame_count = 0 + self.moving = False + + def setup_messaging(self): + self.subscribe('system/loop', self.scan) + + def scan(self): + self.last_results = self.parse_detections() + this_capture = [obj.json_out() for obj in self.last_results] + self.publish('vision/detections', matches=this_capture) + return this_capture + + def parse_detections(self): + self.last_detections = [] + # Stabilization check + if not self.calculate_stabilization(): + self.moving = True + return self.last_detections + elif self.moving: + self.moving = False + self.publish('vision/stable') + # Capture frame from PiCamera2 + try: + frame = self.picam2.capture_array() + except Exception as e: + raise Exception(f"Failed to read frame from camera: {e}") + gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) + faces = self.face_cascade.detectMultiScale( + gray, + scaleFactor=1.1, + minNeighbors=5, + minSize=(30, 30), + ) + # print(f"Found {len(faces)} faces")s + for (x, y, w, h) in faces: + # Haar cascades don't provide confidence, so use 1.0 + self.last_detections.append(Detection((x, y, w, h), 1.0, self)) + return self.last_detections + + def calculate_stabilization(self, threshold=0.70, stable_frames_required=8): + """ + Calculate if the image has stabilized based on frame differences. + Stability is defined as the average pixel difference between consecutive frames + being below a given threshold for a certain number of frames. + :param threshold: Percentage of pixels that must remain stable (default 70%). + :param stable_frames_required: Number of consecutive stable frames to confirm stabilization. + :return: Boolean indicating if the image is stable. + """ + try: + current_frame = self.picam2.capture_array() + except Exception: + return False + + if self.previous_frame is None: + self.previous_frame = current_frame + return False + + frame_diff = cv2.absdiff(current_frame, self.previous_frame) + gray_diff = cv2.cvtColor(frame_diff, cv2.COLOR_BGR2GRAY) + non_zero_count = np.count_nonzero(gray_diff) + total_pixels = gray_diff.size + diff_percentage = non_zero_count / total_pixels + + self.previous_frame = current_frame + + if diff_percentage < threshold: + self.stable_frame_count += 1 + else: + self.stable_frame_count = 0 + + if self.stable_frame_count >= stable_frames_required: + return True + + return False + + @lru_cache + def get_labels(self): + return ["face"] + + def draw_detections(self, frame, detections): + for detection in detections: + x, y, w, h = detection.box + cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2) + label = f"face ({detection.conf:.2f})" + cv2.putText(frame, label, (x, y - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1) + return frame + + @staticmethod + def get_args(): + parser = argparse.ArgumentParser() + parser.add_argument("--camera", type=int, default=0, help="Camera device index (e.g. 0 for /dev/video0)") + # No model argument needed for Haar cascade + return parser.parse_args() + +if __name__ == "__main__": + mycam = Vision() + while True: + detections = mycam.scan() + # Optionally display the detections in a window + try: + frame = mycam.picam2.capture_array() + except Exception: + break + frame = mycam.draw_detections(frame, mycam.last_results) + cv2.imshow("Face Detection", cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + mycam.picam2.stop() + cv2.destroyAllWindows()