Camera (#18)

Add camera sensor and display modules

Author: jebking

README.md

@@ -2,11 +2,11 @@
 
 Code for The Harvard Undergraduate Robotics Club's MATE ROV team.
 
-The code consist of separate modules that are run on either the ROV or on a ground base. Inter-module communications are orchestrated by the Server (server.py). Modules send messages to the server and subscribe to certain message types. When the server receives a message of a certain type, it forwards it to all the modules that have subscribed to that message type.
+The code consist of separate modules that are run on either the ROV or on a ground base. Inter-module communications are orchestrated by the Server (`server.py`). Modules send messages to the server and subscribe to certain message types. When the server receives a message of a certain type, it forwards it to all the modules that have subscribed to that message type.
 
 ## How to run
 
-1. Execute ./server.py
+1. Execute `./server.py`
 2. Execute all of your modules
 
 ## Adding new modules
@@ -26,10 +26,10 @@ The code consist of separate modules that are run on either the ROV or on a grou
     	    protoc -I=./ --python_out=./ ./second.proto
         ```
     - Run the make command in the comm folder.
-    - In messages/__init__.py do the following:
+    - In `messages/__init__.py` do the following:
         - Import your new compiled buffer.
         - Add a message type enum for your new buffer.
-        - Add the new message type and the associated buffer to message_buffers.
+        - Add the new message type and the associated buffer to `message_buffers`.
         - Example:
             - Before:
             ```
@@ -58,4 +58,4 @@ The code consist of separate modules that are run on either the ROV or on a grou
                 MsgType.SECOND: SecondMsg
             }
             ```
-2. Make a new module class that inherits from robomodules.ProtoModule. Look at MockGuiModule.py and MockSensorModule for examples on modules
+2. Make a new module class that inherits from `robomodules.ProtoModule`. Look at `MockGuiModule.py` and `MockSensorModule` for examples on modules.

cameraDisplayModule.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+
+import os
+import cv2
+import robomodules as rm
+from messages import message_buffers, MsgType
+import pickle
+
+ADDRESS = os.environ.get("BIND_ADDRESS","localhost")
+PORT = os.environ.get("BIND_PORT", 11297)
+
+FREQUENCY = 0
+
+class CameraDisplayModule(rm.ProtoModule):
+    def __init__(self, addr, port):
+        self.subscriptions = [MsgType.CAMERA_FRAME_MSG]
+        super().__init__(addr, port, message_buffers, MsgType, FREQUENCY, self.subscriptions)
+        self.frame = None
+
+    def msg_received(self, msg, msg_type):
+        # This gets called whenever any message is received
+        # We receive pickled frames here.
+        if msg_type == MsgType.CAMERA_FRAME_MSG:
+            self.frame = msg.cameraFrame
+        self._display_serialized_image()
+        
+
+    def tick(self):
+        # this function will get called in a loop with FREQUENCY frequency
+        # process the serialized frame
+        return
+
+    def _display_serialized_image(self):
+        if self.frame == None:
+            print('frame == None')
+            return
+        frame = pickle.loads(self.frame)
+        # Our operations on the frame come here
+        # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        # Display the resulting frame
+        cv2.imshow('frame', frame)
+        if cv2.waitKey(1) & 0xFF == ord('q'):
+            return
+
+def main():
+    module = CameraDisplayModule(ADDRESS, PORT)
+    module.run()
+
+if __name__ == "__main__":
+    main()

cameraSensorModule.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+
+import os, random
+import robomodules as rm
+from messages import *
+import numpy as np
+import cv2
+import pickle
+
+ADDRESS = os.environ.get("BIND_ADDRESS","localhost")
+PORT = os.environ.get("BIND_PORT", 11297)
+
+FREQUENCY = 30
+
+class CameraSensorModule(rm.ProtoModule):
+    def __init__(self, addr, port, camera_port):
+        super().__init__(addr, port, message_buffers, MsgType, FREQUENCY)
+        self.cam = cv2.VideoCapture(camera_port)
+
+    def msg_received(self, msg, msg_type):
+        # This gets called whenever any message is received
+        # This module only sends data, so we ignore incoming messages
+        return
+
+    def tick(self):
+        # this function will get called in a loop with FREQUENCY frequency
+        # gets the camera feed, puts it into the message
+        msg = CameraFrameMsg()
+        ret, frame = self.cam.read()
+        msg.cameraFrame = pickle.dumps(frame)
+        msg = msg.SerializeToString()
+        self.write(msg, MsgType.CAMERA_FRAME_MSG)
+
+def main():
+    module = CameraSensorModule(ADDRESS, PORT, 0)
+    module.run()
+
+if __name__ == "__main__":
+    main()

messages/Makefile

@@ -1,2 +1,3 @@
-protobuf: mockMsg.proto
+protobuf: mockMsg.proto cameraFrameMsg.proto
 	protoc -I=./ --python_out=./ ./mockMsg.proto
+	protoc -I=./ --python_out=./ ./cameraFrameMsg.proto

messages/__init__.py

@@ -1,12 +1,15 @@
 from enum import Enum
 from .mockMsg_pb2 import MockMsg
+from .cameraFrameMsg_pb2 import CameraFrameMsg
 
 class MsgType(Enum):
     MOCK_MSG = 0
+    CAMERA_FRAME_MSG = 1
 
 message_buffers = {
-    MsgType.MOCK_MSG: MockMsg
+    MsgType.MOCK_MSG: MockMsg,
+    MsgType.CAMERA_FRAME_MSG: CameraFrameMsg
 }
 
 
-__all__ = ['MsgType', 'message_buffers', 'MockMsg']
+__all__ = ['MsgType', 'message_buffers', 'MockMsg', 'CameraFrameMsg']

messages/cameraFrameMsg.proto

@@ -0,0 +1,8 @@
+syntax = "proto2";
+
+package mateROV;
+
+message CameraFrameMsg {
+
+  required bytes cameraFrame = 1;
+}