From 3f2ab2356338ab31a02b327a7160a68c90f1f3e9 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Tue, 8 Jan 2019 01:24:01 -0800
Subject: [PATCH] Move default multiCameraServer from allwpilib to tools

---
 deps/04-copy.sh                  |   4 +-
 deps/tools/Makefile              |  22 ++++
 deps/tools/multiCameraServer.cpp | 190 +++++++++++++++++++++++++++++++
 stage2/01-sys-tweaks/01-run.sh   |   3 +
 4 files changed, 218 insertions(+), 1 deletion(-)
 create mode 100644 deps/tools/multiCameraServer.cpp

diff --git a/deps/04-copy.sh b/deps/04-copy.sh
index dec4d2a..2b73bc1 100755
--- a/deps/04-copy.sh
+++ b/deps/04-copy.sh
@@ -19,6 +19,8 @@ sh -c 'cd examples && zip -r - python-multiCameraServer' > ${DEST}/python-multiC
 cp tools/setuidgids ${DEST}/
 cp tools/_cscore.so ${DEST}/_cscore.cpython-35m-arm-linux-gnueabihf.so
 cp tools/_cscore.so.debug ${DEST}/
+cp tools/multiCameraServer ${DEST}/
+cp tools/multiCameraServer.debug ${DEST}/
 cp tools/rpiConfigServer ${DEST}/
 cp tools/rpiConfigServer.debug ${DEST}/
 
@@ -72,4 +74,4 @@ sh -c 'cd allwpilib/cscore/src/main/native/include && tar czf - .' > ${DEST}/csc
 sh -c 'cd allwpilib/ntcore/src/main/native/include && tar czf - .' > ${DEST}/ntcore-include.tar.gz
 sh -c 'cd allwpilib/cameraserver/src/main/native/include && tar czf - cameraserver vision' > ${DEST}/cameraserver-include.tar.gz
 
-sh -c 'cd 03-build/allwpilib-static/bin && tar czf - cscore_* multiCameraServer* netconsoleTee*' > ${DEST}/wpilib-bin.tar.gz
+sh -c 'cd 03-build/allwpilib-static/bin && tar czf - cscore_* netconsoleTee*' > ${DEST}/wpilib-bin.tar.gz
diff --git a/deps/tools/Makefile b/deps/tools/Makefile
index b577e56..05b1e8a 100644
--- a/deps/tools/Makefile
+++ b/deps/tools/Makefile
@@ -4,6 +4,7 @@ WPILIB_SRC?=../allwpilib
 WPILIB_BUILD?=../03-build/allwpilib-build
 WPILIB_STATIC_BUILD?=../03-build/allwpilib-static
 OPENCV_INSTALL?=../03-build/opencv-build/install
+OPENCV_STATIC_INSTALL?=../03-build/opencv-static/install
 EXEC_HOME?=/home/pi
 FRC_JSON?=/boot/frc.json
 DHCPCD_CONF?=/boot/dhcpcd.conf
@@ -42,6 +43,27 @@ _cscore.so: ../robotpy-cscore/src/_cscore.cpp ../robotpy-cscore/src/ndarray_conv
 	${COMPILER}strip -g $@
 	${COMPILER}objcopy --add-gnu-debuglink=$@.debug $@
 
+multiCameraServer: multiCameraServer.cpp
+	${COMPILER}g++ -pthread -g -o $@ \
+	    -I${OPENCV_INSTALL}/include \
+	    -I${WPILIB_SRC}/wpiutil/src/main/native/include \
+	    -I${WPILIB_SRC}/cameraserver/src/main/native/include \
+	    -I${WPILIB_SRC}/cscore/src/main/native/include \
+	    -I${WPILIB_SRC}/ntcore/src/main/native/include \
+	    -L${WPILIB_STATIC_BUILD}/lib \
+	    -L${OPENCV_STATIC_INSTALL}/lib \
+	    -L${OPENCV_STATIC_INSTALL}/share/OpenCV/3rdparty/lib \
+	    $< \
+	    -lcameraserver \
+	    -lcscore \
+	    -lntcore \
+	    -lwpiutil \
+	    -lopencv_highgui -lopencv_imgcodecs -lopencv_imgproc -lopencv_core \
+	    -ltegra_hal -llibpng -llibjpeg-turbo -lzlib
+	${COMPILER}objcopy --only-keep-debug $@ $@.debug
+	${COMPILER}strip -g $@
+	${COMPILER}objcopy --add-gnu-debuglink=$@.debug $@
+
 RPICONFIGSERVER_OBJS= \
     rpiConfigServer_src/main.o \
     rpiConfigServer_src/Application.o \
diff --git a/deps/tools/multiCameraServer.cpp b/deps/tools/multiCameraServer.cpp
new file mode 100644
index 0000000..dbfc7b6
--- /dev/null
+++ b/deps/tools/multiCameraServer.cpp
@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cstdio>
+#include <string>
+#include <vector>
+
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/StringRef.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+#include "cameraserver/CameraServer.h"
+
+/*
+   JSON format:
+   {
+       "team": <team number>,
+       "ntmode": <"client" or "server", "client" if unspecified>
+       "cameras": [
+           {
+               "name": <camera name>
+               "path": <path, e.g. "/dev/video0">
+               "pixel format": <"MJPEG", "YUYV", etc>   // optional
+               "width": <video mode width>              // optional
+               "height": <video mode height>            // optional
+               "fps": <video mode fps>                  // optional
+               "brightness": <percentage brightness>    // optional
+               "white balance": <"auto", "hold", value> // optional
+               "exposure": <"auto", "hold", value>      // optional
+               "properties": [                          // optional
+                   {
+                       "name": <property name>
+                       "value": <property value>
+                   }
+               ]
+           }
+       ]
+   }
+ */
+
+#ifdef __RASPBIAN__
+static const char* configFile = "/boot/frc.json";
+#else
+static const char* configFile = "frc.json";
+#endif
+
+namespace {
+
+unsigned int team;
+bool server = false;
+
+struct CameraConfig {
+  std::string name;
+  std::string path;
+  wpi::json config;
+};
+
+std::vector<CameraConfig> cameras;
+
+wpi::raw_ostream& ParseError() {
+  return wpi::errs() << "config error in '" << configFile << "': ";
+}
+
+bool ReadCameraConfig(const wpi::json& config) {
+  CameraConfig c;
+
+  // name
+  try {
+    c.name = config.at("name").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    ParseError() << "could not read camera name: " << e.what() << '\n';
+    return false;
+  }
+
+  // path
+  try {
+    c.path = config.at("path").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    ParseError() << "camera '" << c.name
+                 << "': could not read path: " << e.what() << '\n';
+    return false;
+  }
+
+  c.config = config;
+
+  cameras.emplace_back(std::move(c));
+  return true;
+}
+
+bool ReadConfig() {
+  // open config file
+  std::error_code ec;
+  wpi::raw_fd_istream is(configFile, ec);
+  if (ec) {
+    wpi::errs() << "could not open '" << configFile << "': " << ec.message()
+                << '\n';
+    return false;
+  }
+
+  // parse file
+  wpi::json j;
+  try {
+    j = wpi::json::parse(is);
+  } catch (const wpi::json::parse_error& e) {
+    ParseError() << "byte " << e.byte << ": " << e.what() << '\n';
+    return false;
+  }
+
+  // top level must be an object
+  if (!j.is_object()) {
+    ParseError() << "must be JSON object\n";
+    return false;
+  }
+
+  // team number
+  try {
+    team = j.at("team").get<unsigned int>();
+  } catch (const wpi::json::exception& e) {
+    ParseError() << "could not read team number: " << e.what() << '\n';
+    return false;
+  }
+
+  // ntmode (optional)
+  if (j.count("ntmode") != 0) {
+    try {
+      auto str = j.at("ntmode").get<std::string>();
+      wpi::StringRef s(str);
+      if (s.equals_lower("client")) {
+        server = false;
+      } else if (s.equals_lower("server")) {
+        server = true;
+      } else {
+        ParseError() << "could not understand ntmode value '" << str << "'\n";
+      }
+    } catch (const wpi::json::exception& e) {
+      ParseError() << "could not read ntmode: " << e.what() << '\n';
+    }
+  }
+
+  // cameras
+  try {
+    for (auto&& camera : j.at("cameras")) {
+      if (!ReadCameraConfig(camera)) return false;
+    }
+  } catch (const wpi::json::exception& e) {
+    ParseError() << "could not read cameras: " << e.what() << '\n';
+    return false;
+  }
+
+  return true;
+}
+
+void StartCamera(const CameraConfig& config) {
+  wpi::outs() << "Starting camera '" << config.name << "' on " << config.path
+              << '\n';
+  auto camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(
+      config.name, config.path);
+
+  camera.SetConfigJson(config.config);
+}
+}  // namespace
+
+int main(int argc, char* argv[]) {
+  if (argc >= 2) configFile = argv[1];
+
+  // read configuration
+  if (!ReadConfig()) return EXIT_FAILURE;
+
+  // start NetworkTables
+  auto ntinst = nt::NetworkTableInstance::GetDefault();
+  if (server) {
+    wpi::outs() << "Setting up NetworkTables server\n";
+    ntinst.StartServer();
+  } else {
+    wpi::outs() << "Setting up NetworkTables client for team " << team << '\n';
+    ntinst.StartClientTeam(team);
+  }
+
+  // start cameras
+  for (auto&& camera : cameras) StartCamera(camera);
+
+  // loop forever
+  for (;;) std::this_thread::sleep_for(std::chrono::seconds(10));
+}
diff --git a/stage2/01-sys-tweaks/01-run.sh b/stage2/01-sys-tweaks/01-run.sh
index f98ae15..9ec7c80 100755
--- a/stage2/01-sys-tweaks/01-run.sh
+++ b/stage2/01-sys-tweaks/01-run.sh
@@ -87,6 +87,9 @@ install -m 644 files/ld.so.conf.d/*.conf "${ROOTFS_DIR}/etc/ld.so.conf.d/"
 
 install -v -d "${ROOTFS_DIR}/usr/local/frc/bin"
 
+install -m 755 extfiles/multiCameraServer "${ROOTFS_DIR}/usr/local/frc/bin/"
+install -m 644 extfiles/multiCameraServer.debug "${ROOTFS_DIR}/usr/local/frc/bin/"
+
 cat extfiles/wpilib-bin.tar.gz | sh -c 'cd ${ROOTFS_DIR}/usr/local/frc/bin/ && tar xzf -'
 
 install -v -d "${ROOTFS_DIR}/usr/local/frc/lib"