openpose PTZ modifications

Dockerfile

@@ -8,7 +8,7 @@ RUN apt-get update && \
     libgoogle-glog-dev libboost-all-dev libcaffe-cuda-dev libhdf5-dev libatlas-base-dev nano python3-setuptools
 
 #for python api
-RUN pip3 install --upgrade pip && pip3 install numpy opencv-python paho-mqtt
+RUN pip3 install --upgrade pip && pip3 install numpy opencv-python paho-mqtt v4l2-python3
 
 #replace cmake as old version has CUDA variable bugs
 RUN wget https://github.com/Kitware/CMake/releases/download/v3.16.0/cmake-3.16.0-Linux-x86_64.tar.gz && \
@@ -37,17 +37,17 @@ RUN cd /openpose/build && \
 ENV LD_LIBRARY_PATH=/openpose/build/python/openpose:/usr/lib
 
 # Installing NDI and compiling python bindings
-COPY NDI/libndi.so.4 /usr/lib/libndi.so
-COPY NDI/include/* /usr/include/
-
-RUN cd / && \
-    git clone https://github.com/buresu/ndi-python.git && \
-    cd /ndi-python && \
-    git submodule init && git submodule update && \
-    cd lib/pybind11 && git checkout v2.9.1 && \
-    cd /ndi-python && mkdir build && cd build && \
-    cmake .. && cmake --build . --config Release && \
-    cp NDIlib*.so /usr/lib/python3.6/NDIlib.so
+#COPY NDI/libndi.so.4 /usr/lib/libndi.so
+#OPY NDI/include/* /usr/include/
+
+#RUN cd / && \
+#    git clone https://github.com/buresu/ndi-python.git && \
+#    cd /ndi-python && \
+#    git submodule init && git submodule update && \
+#    cd lib/pybind11 && git checkout v2.9.1 && \
+#    cd /ndi-python && mkdir build && cd build && \
+#    cmake .. && cmake --build . --config Release && \
+#    cp NDIlib*.so /usr/lib/python3.6/NDIlib.so
 
 
 WORKDIR /ptztrack

README.md

@@ -2,7 +2,13 @@
 
 Uses [openpose](https://github.com/CMU-Perceptual-Computing-Lab/openpose) to identify people in an video feed, from v4l2 or NDI, and issue VISCA over IP PTZ commands to a networked camera. MQTT can be used to issue commands to turn the control on/off.
 
-Currently tested and used with a BirdDog P200 using [`ffmpeg` with NDI support](https://framagit.org/tytan652/ffmpeg-ndi-patch/) to feed a v4l2 loopback device on Ubuntu. [Companion](https://github.com/bitfocus/companion) on a StreamDeck, and its' MQTT module, is used to toggle automatic control on/off. It's setup and run in Docker for tidiness sake. NDI testing is ongoing.
+## Tested cameras:
+
+* BirdDog P200 using [`ffmpeg` with NDI support](https://framagit.org/tytan652/ffmpeg-ndi-patch/) to feed a v4l2 loopback device on Ubuntu. NDI testing is ongoing. 
+
+* Vaddio ConferenceSHOT 10 running newest firmware with VISCA IP support
+
+MQTT is supported to control automatic control on/off. This can be done with [Companion](https://github.com/bitfocus/companion) on a StreamDeck, and its' MQTT module. 
 
 It tracks the middle of all people it finds, so it probably won't do much if there are several people visible.
 
@@ -49,7 +55,7 @@ Will read from `/dev/video0` and automatically start controlling the camera at t
 
 ```
 $ docker run --gpus all --name ptztrack --restart unless-stopped -it \
-    --device /dev/video0 ptztrack:11.2 10.1.1.174
+    --device /dev/video0 ptzcontrol:11.2 10.1.1.174
 ```
 
 If all goes well it should start up with no errors, print out in the console when it's moving, and actually move the camera.

movecontrol.py

@@ -6,6 +6,11 @@ class Move(IntEnum):
     STOP = 0
     LEFT = 1
     RIGHT = 2
+    UP = 3
+    DOWN = 4
+    ZIN = 5
+    ZOUT = 6
+    ZSTOP = 7
 
 
 class BaseMoveControl:
@@ -42,6 +47,11 @@ class ViscaMoveControl(BaseMoveControl):
     LEFT = " 01 03 FF"
     RIGHT = " 02 03 FF"
     STOP = " 03 03 FF"
+    UP = " 03 02 FF"
+    DOWN = " 03 01 FF"
+    ZIN = "81 01 04 07 22 FF"
+    ZOUT = "81 01 04 07 03 FF"
+    ZSTOP = "81 01 04 07 00 FF"
 
     def __init__(self, args: Namespace):
         import socket
@@ -68,6 +78,16 @@ def do_move(self):
             self.send_packet(self.make_move_str(self.LEFT))
         elif self.direction == Move.RIGHT:
             self.send_packet(self.make_move_str(self.RIGHT))
+        elif self.direction == Move.UP:
+            self.send_packet(self.make_move_str(self.UP))
+        elif self.direction == Move.DOWN:
+            self.send_packet(self.make_move_str(self.DOWN))
+        elif self.direction == Move.ZIN:
+            self.send_packet(self.ZIN)
+        elif self.direction == Move.ZOUT:
+            self.send_packet(self.ZOUT)
+        elif self.direction == Move.ZSTOP:
+            self.send_packet(self.ZSTOP)
         else:
             self.send_packet(self.make_move_str(self.STOP))
 
@@ -88,7 +108,7 @@ def reset_sequence_number(self):
         """
 
         self.socket.sendto(
-            bytearray.fromhex(self.SEQUENCE_RESET), (self.host, self.port)
+            bytearray.fromhex(self.SEQUENCE_RESET), (self.host, int(self.port))
         )
         self.sequence_number = 1
 
@@ -122,4 +142,4 @@ def send_packet(self, command: str):
         )
 
         self.sequence_number += 1
-        self.socket.sendto(message, (self.host, self.port))
+        self.socket.sendto(message, (self.host, int(self.port)))

ptzcontroller.py

@@ -1,6 +1,8 @@
 import cv2, time, argparse, time
 import pyopenpose as op
 import paho.mqtt.client as mqtt
+import os, fcntl
+import v4l2
 from enum import IntEnum
 from signal import signal, SIGINT
 from sys import exit
@@ -21,6 +23,24 @@ class PTZTrack:
     def __init__(self, args):
         self.args = args
         self.move = ViscaMoveControl(self.args)
+        self.zooming = False
+        self.odevice = open("/dev/video1", "wb")
+
+        # V4L2 Setup
+        cap                         = cv2.VideoCapture(0)
+        ret, im                     = cap.read()
+        height2, width2, channels2  = im.shape
+
+        format                      = v4l2.v4l2_format()
+        format.type                 = v4l2.V4L2_BUF_TYPE_VIDEO_OUTPUT
+        format.fmt.pix.field        = v4l2.V4L2_FIELD_NONE
+        format.fmt.pix.pixelformat  = v4l2.V4L2_PIX_FMT_BGR24
+        format.fmt.pix.width        = 1280
+        format.fmt.pix.height       = 720
+        format.fmt.pix.bytesperline = 1280 * channels2
+        format.fmt.pix.sizeimage    = 1280 * 720 * channels2
+
+        print ("set format result (0 is good):{}".format(fcntl.ioctl(self.odevice, v4l2.VIDIOC_S_FMT, format)))
 
         # OpenPose Setup
         params = dict()
@@ -73,13 +93,20 @@ def mqtt_message(self, client, userdata, message):
             self.control_camera = not self.control_camera
             self.mqtt_publish_state()
 
-    def calculate_speed(self, smin: int, val: int, smax: int):
+    def calculate_pan_speed(self, smin: int, val: int, smax: int):
         speed_ratio = (val - smin) / smax
         speed = int(
             ((self.args.speed_max - self.args.speed_min) * speed_ratio)
             + self.args.speed_min
         )
         return speed
+    def calculate_tilt_speed(self, smin: int, val: int, smax: int):
+        speed_ratio = (val - smin) / smax
+        speed = int(
+            ((self.args.speed_max - (self.args.speed_min * 1.75)) * speed_ratio)
+            + self.args.speed_min
+        )
+        return speed
 
     def move_state_str(self):
         return "on" if self.control_camera else "off"
@@ -142,15 +169,17 @@ def calculate_edges(self, frame_shape):
         r_edge = frame_shape[1] - l_edge
         height = frame_shape[0]
         width = frame_shape[1]
+        lower_edge = int(frame_shape[0] * .45)
+        upper_edge = frame_shape[0] - lower_edge
 
         bounding = [frame_shape[1], height, 0, 0]
 
-        return l_edge, r_edge, height, width, bounding
+        return l_edge, r_edge, height, width, lower_edge, upper_edge, bounding
 
     def process_datum_keypoints(self, frame, datum):
         regions = []
 
-        if datum.poseKeypoints:
+        if datum.poseKeypoints is not None and len(datum.poseKeypoints) > 0:
             for i in range(0, datum.poseKeypoints.shape[0]):
                 p = self.get_keypoints_rectangle(datum.poseKeypoints[i], 0.1)
                 regions.append([p[0], p[1], p[2] - p[0], p[3] - p[1]])
@@ -171,8 +200,21 @@ def calculate_boundaries(self, bounding, regions):
 
         return bounding
 
+
+
     def main_loop(self):
         bounding = []
+        val1   = 10
+        val2   = 10
+        val3   = 10
+        val4   = 10
+        val5   = 10
+        val6   = 10
+        val7   = 10
+        val8   = 10
+        val9   = 10
+        val10  = 10
+
 
         self.move.set_direction(Move.STOP)
         last_direction = self.move.direction
@@ -188,9 +230,11 @@ def main_loop(self):
             # If there is no video data
             if not check:
                 # sleep momentarily so we can't waste time in an endless loop
+                print("No Video Data!")
                 time.sleep(0.01)
                 continue
 
+
             # Publish control state occasionally
             frame_count += 1
             if self.args.mqtt and frame_count == 20:
@@ -201,7 +245,10 @@ def main_loop(self):
             if not self.control_camera:
                 continue
 
-            l_edge, r_edge, height, width, bounding = self.calculate_edges(frame.shape)
+            l_edge, r_edge, height, width, lower_edge, upper_edge, bounding = self.calculate_edges(frame.shape)
+##            lower_edge = (frame_shape[0] * 0.45)
+##            upper_edge = (frame_shape[0] - lower_edge)
+
 
             # Pass the frame data to openpose
             openpose_datum = op.Datum()
@@ -216,14 +263,36 @@ def main_loop(self):
                 # calculate the bounding boxes of all the people
                 bounding = self.calculate_boundaries(bounding, regions)
 
+                val1 = val2
+                val2 = val3
+                val3 = val4
+                val4 = val5
+                val5 = val6
+                val6 = val7
+                val7 = val8
+                val8 = val9
+                val9 = val10
+                val10 = (bounding[Edge.TOP] - bounding[Edge.BOTTOM])
+                val11 = (val6 + val7 + val8 + val9 + val10) / 5 
+                val11 = abs(val11)
+                recty1 = (height - val11) * 0.5
+                recty2 = recty1 + val11
+                cv2.rectangle(
+                    frame, 
+                    (20, int(recty1)), 
+                    (20, int(recty2)), 
+                    (255, 255, 0),
+                    4,
+                )
+
                 # Calculate the middle of the box
                 lrmiddle = int(
                     ((bounding[Edge.RIGHT] - bounding[Edge.LEFT]) / 2)
                     + bounding[Edge.LEFT]
                 )
                 udmiddle = int(
-                    ((bounding[Edge.BOTTOM] - bounding[Edge.TOP]) / 2)
-                    + bounding[Edge.TOP]
+                    ((bounding[Edge.TOP] - bounding[Edge.BOTTOM]) / 2)
+                    + bounding[Edge.BOTTOM]
                 )
 
                 # Draw the bounding boxes and middle point
@@ -242,20 +311,47 @@ def main_loop(self):
                     4,
                 )
                 cv2.rectangle(frame, (l_edge, 0), (r_edge, height), (255, 0, 0), 4)
+                cv2.rectangle(frame, (0, lower_edge), (width, upper_edge), (0, 255, 0), 4)
 
                 # Calculate the move speed as a ratio of the distance between the bounding box edge and frame edge.
+                print("Average Height: " + str(val11))
+                print("Zout threshold: " + str(.85 * height))
                 if lrmiddle < l_edge:
                     self.move.set_speed(
-                        self.calculate_speed(0, l_edge - lrmiddle, l_edge)
+                        self.calculate_pan_speed(0, l_edge - lrmiddle, l_edge)
                     )
                     self.move.set_direction(Move.LEFT)
                 elif lrmiddle > r_edge:
-                    self.move.set_speed(self.calculate_speed(r_edge, lrmiddle, width))
+                    self.move.set_speed(
+                         self.calculate_pan_speed(0, lrmiddle - r_edge, l_edge)
+                    )
                     self.move.set_direction(Move.RIGHT)
+                elif udmiddle < lower_edge:
+                    self.move.set_speed(
+                        self.calculate_tilt_speed(0, lower_edge - udmiddle, lower_edge)
+                    )
+                    self.move.set_direction(Move.DOWN)
+                elif udmiddle > upper_edge:
+                    self.move.set_speed(
+                         self.calculate_tilt_speed(0, udmiddle - upper_edge, lower_edge)
+                    )
+                    self.move.set_direction(Move.UP)
+                elif val11 >= (.75 * height):
+                      self.move.set_direction(Move.ZOUT)
+                      self.zooming = True
+
+                elif val11 <= (.55 * height):
+                      self.move.set_direction(Move.ZIN)
+                      self.zooming = True
                 else:
+                    if self.zooming:
+                        self.move.set_direction(Move.ZSTOP)
+                        self.move.do_move()
+                        self.zooming = False
                     self.move.set_speed(self.args.speed_min)
                     self.move.set_direction(Move.STOP)
             else:
+
                 self.move.set_direction(Move.STOP)
 
             # If either the speed or direction have changed then send the move command to the camera
@@ -269,6 +365,8 @@ def main_loop(self):
             if self.args.ui:
                 if self.show_ui(frame):
                     break
+            #write frame to v4l2 loopback
+            self.odevice.write(frame)
 
 
 if __name__ == "__main__":