FacePointerServo.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee52ccb7",
   "metadata": {},
   "outputs": [],
   "source": [
    "from aiy.vision.inference import CameraInference\n",
    "from aiy.vision.models import face_detection\n",
    "from aiy.vision.streaming.server import StreamingServer\n",
    "from aiy.vision.streaming import svg\n",
    "from aiy.leds import Leds, Color\n",
    "from gpiozero import Servo\n",
    "from aiy.pins import PIN_A\n",
    "\n",
    "from picamera import PiCamera\n",
    "from IPython.display import Image, display, clear_output\n",
    "\n",
    "import contextlib\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6269c4f0",
   "metadata": {},
   "source": [
    "### Face Pointer\n",
    "\n",
    "We are going to use the servo motor to point at the face that was detected. We will use the bounding box to determine the angle. And remember if your joy detector is running, you need to turn it off using the commands\n",
    "\n",
    "```\n",
    "sudo systemctl stop joy_detection_demo.service\n",
    "```\n",
    "\n",
    "The code below initializes the servo assuming you connected it to PIN_A. Ignore the notics when runnig the code. Also it **seems the only way to run this cell twice is to restart the kernel** (still debugging), so run it once and then change and play wioth the code below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a5f4fb5",
   "metadata": {},
   "outputs": [],
   "source": [
    "servo = Servo(PIN_A)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c67328dc",
   "metadata": {},
   "source": [
    "I added some code here that is called to create the overlay - basically the box around the face and the score above. You can customize it and/or add information you want to overlay on the camera feed. **This is not needed for the servo.**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9a383051",
   "metadata": {},
   "outputs": [],
   "source": [
    "def svg_overlay(faces, frame_size):\n",
    "\n",
    "    JOY_COLOR = (255, 70, 0)\n",
    "    SAD_COLOR = (0, 0, 64)\n",
    "\n",
    "    width, height = frame_size\n",
    "    doc = svg.Svg(width=width, height=height)\n",
    "\n",
    "    for face in faces:\n",
    "        x, y, w, h = face.bounding_box\n",
    "        fcol='rgb'+str(Color.blend(JOY_COLOR, SAD_COLOR, face.joy_score))\n",
    "        doc.add(svg.Rect(x=int(x), y=int(y), width=int(w), height=int(h), rx=10, ry=10,\n",
    "                         fill_opacity=0.3 * face.face_score,\n",
    "                         style='fill:'+fcol+';stroke:white;stroke-width:4px'))\n",
    "\n",
    "        doc.add(svg.Text('Joy: %.2f' % face.joy_score, x=x, y=y-10, fill='red', font_size=30))\n",
    "\n",
    "    return str(doc)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9de67ca5",
   "metadata": {},
   "source": [
    "#### Main loop\n",
    "\n",
    "Here is our main loop based on the code we used last time. Look at the comments on what was changed. Once we see a face, we call the routine `pointFace` to move the servo to point in the direction of the face. We also added the streaming back in, to while this cell runs, you can connect to http://orcspi-vis.local:4664 and see the stream."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "032279c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "def pointFace(box, width):\n",
    "    \n",
    "    x, y, w, h = box\n",
    "    middle = x + w/2\n",
    "    offset = 2*middle/width - 1\n",
    "\n",
    "    print (\"Offset: {offset:.2f}, Middle: {middle}\".format(offset=offset, middle=middle))\n",
    "    \n",
    "    return -offset"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "06726e24",
   "metadata": {},
   "source": [
    "And the main loop we already know so well :)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "01f89e8d",
   "metadata": {},
   "outputs": [],
   "source": [
    "with contextlib.ExitStack() as stack:\n",
    "    leds   = stack.enter_context(Leds())\n",
    "    camera = stack.enter_context(PiCamera(sensor_mode=4, resolution=(820, 616)))\n",
    "\n",
    "    # This starts and runs the streaming of the camera\n",
    "    server = stack.enter_context(StreamingServer(camera))  \n",
    "\n",
    "    print (\"Loading model - hold on ..\")\n",
    "    \n",
    "    # Move the servo to low (-1)\n",
    "    servo.mid()\n",
    "    \n",
    "    # Do inference on VisionBonnet\n",
    "    with CameraInference(face_detection.model()) as inference:\n",
    "        try:   \n",
    "            for result in inference.run():\n",
    "                leds.update(Leds.rgb_on(Color.RED))\n",
    "                faces = face_detection.get_faces(result)\n",
    "                \n",
    "                # This sends the overlay (boxes) to add to the camera stream\n",
    "                server.send_overlay(svg_overlay(faces, (result.width, result.height)))\n",
    "\n",
    "                if len(faces) >= 1:\n",
    "                    clear_output(wait=True)     \n",
    "                    servo.value = pointFace(faces[0].bounding_box, result.width)\n",
    "                    leds.update(Leds.rgb_on(Color.GREEN))\n",
    "                                           \n",
    "        except KeyboardInterrupt:\n",
    "            print(\"Interrupted ..\")\n",
    "            \n",
    "    leds.update(Leds.rgb_off())\n",
    "    \n",
    "    # Servo back to the middle upon ending\n",
    "    servo.mid()\n",
    "    \n",
    "    print(\"Done\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0fd1edd4",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}