From bfe15245a625d8e30351fc1096d2df1ca71210c3 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Fri, 7 Dec 2018 23:57:37 -0800
Subject: [PATCH] Add multiCameraServer executable (#1422)

This standalone CameraServer reads its configuration from a json file.
---
 cameraserver/multiCameraServer/build.gradle   |  62 +++++
 .../multiCameraServer/src/main/java/Main.java | 211 ++++++++++++++++++
 .../src/main/native/cpp/main.cpp              | 190 ++++++++++++++++
 settings.gradle                               |   1 +
 4 files changed, 464 insertions(+)
 create mode 100644 cameraserver/multiCameraServer/build.gradle
 create mode 100644 cameraserver/multiCameraServer/src/main/java/Main.java
 create mode 100644 cameraserver/multiCameraServer/src/main/native/cpp/main.cpp

diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle
new file mode 100644
index 0000000000..c49cfb00c3
--- /dev/null
+++ b/cameraserver/multiCameraServer/build.gradle
@@ -0,0 +1,62 @@
+plugins {
+    id 'java'
+    id 'application'
+    id 'cpp'
+    id 'visual-studio'
+}
+
+apply plugin: 'edu.wpi.first.NativeUtils'
+
+apply from: "${rootDir}/shared/config.gradle"
+
+ext {
+    staticCvConfigs = [multiCameraServerCpp: []]
+    useJava = true
+    useCpp = true
+    skipDev = true
+}
+
+apply from: "${rootDir}/shared/opencv.gradle"
+
+mainClassName = 'Main'
+
+apply plugin: 'com.github.johnrengelman.shadow'
+
+repositories {
+    mavenCentral()
+}
+
+dependencies {
+    compile 'com.google.code.gson:gson:2.8.5'
+
+    compile project(':wpiutil')
+    compile project(':ntcore')
+    compile project(':cscore')
+    compile project(':cameraserver')
+}
+
+model {
+    components {
+        multiCameraServerCpp(NativeExecutableSpec) {
+            targetBuildTypes 'release'
+            sources {
+                cpp {
+                    source {
+                        srcDirs = ['src/main/native/cpp']
+                        includes = ['**/*.cpp']
+                    }
+                    exportedHeaders {
+                        srcDirs = ['src/main/native/include']
+                        includes = ['**/*.h']
+                    }
+                }
+            }
+            binaries.all { binary ->
+                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
+                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                    lib project: ':cscore', library: 'cscore', linkage: 'static'
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+            }
+        }
+    }
+}
diff --git a/cameraserver/multiCameraServer/src/main/java/Main.java b/cameraserver/multiCameraServer/src/main/java/Main.java
new file mode 100644
index 0000000000..8bcc54d031
--- /dev/null
+++ b/cameraserver/multiCameraServer/src/main/java/Main.java
@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+
+import com.google.gson.Gson;
+import com.google.gson.GsonBuilder;
+import com.google.gson.JsonArray;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParser;
+
+import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+/*
+   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>
+                   }
+               ]
+           }
+       ]
+   }
+ */
+
+public final class Main {
+  private static String configFile = "/boot/frc.json";
+
+  @SuppressWarnings("MemberName")
+  public static class CameraConfig {
+    public String name;
+    public String path;
+    public JsonObject config;
+  }
+
+  public static int team;
+  public static boolean server;
+  public static List<CameraConfig> cameras = new ArrayList<>();
+
+  private Main() {
+  }
+
+  /**
+   * Report parse error.
+   */
+  public static void parseError(String str) {
+    System.err.println("config error in '" + configFile + "': " + str);
+  }
+
+  /**
+   * Read single camera configuration.
+   */
+  public static boolean readCameraConfig(JsonObject config) {
+    CameraConfig cam = new CameraConfig();
+
+    // name
+    JsonElement nameElement = config.get("name");
+    if (nameElement == null) {
+      parseError("could not read camera name");
+      return false;
+    }
+    cam.name = nameElement.getAsString();
+
+    // path
+    JsonElement pathElement = config.get("path");
+    if (pathElement == null) {
+      parseError("camera '" + cam.name + "': could not read path");
+      return false;
+    }
+    cam.path = pathElement.getAsString();
+
+    cam.config = config;
+
+    cameras.add(cam);
+    return true;
+  }
+
+  /**
+   * Read configuration file.
+   */
+  @SuppressWarnings("PMD.CyclomaticComplexity")
+  public static boolean readConfig() {
+    // parse file
+    JsonElement top;
+    try {
+      top = new JsonParser().parse(Files.newBufferedReader(Paths.get(configFile)));
+    } catch (IOException ex) {
+      System.err.println("could not open '" + configFile + "': " + ex);
+      return false;
+    }
+
+    // top level must be an object
+    if (!top.isJsonObject()) {
+      parseError("must be JSON object");
+      return false;
+    }
+    JsonObject obj = top.getAsJsonObject();
+
+    // team number
+    JsonElement teamElement = obj.get("team");
+    if (teamElement == null) {
+      parseError("could not read team number");
+      return false;
+    }
+    team = teamElement.getAsInt();
+
+    // ntmode (optional)
+    if (obj.has("ntmode")) {
+      String str = obj.get("ntmode").getAsString();
+      if ("client".equalsIgnoreCase(str)) {
+        server = false;
+      } else if ("server".equalsIgnoreCase(str)) {
+        server = true;
+      } else {
+        parseError("could not understand ntmode value '" + str + "'");
+      }
+    }
+
+    // cameras
+    JsonElement camerasElement = obj.get("cameras");
+    if (camerasElement == null) {
+      parseError("could not read cameras");
+      return false;
+    }
+    JsonArray cameras = camerasElement.getAsJsonArray();
+    for (JsonElement camera : cameras) {
+      if (!readCameraConfig(camera.getAsJsonObject())) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  /**
+   * Start running the camera.
+   */
+  public static void startCamera(CameraConfig config) {
+    System.out.println("Starting camera '" + config.name + "' on " + config.path);
+    VideoSource camera = CameraServer.getInstance().startAutomaticCapture(
+        config.name, config.path);
+
+    Gson gson = new GsonBuilder().create();
+
+    camera.setConfigJson(gson.toJson(config.config));
+  }
+
+  /**
+   * Main.
+   */
+  public static void main(String... args) {
+    if (args.length > 0) {
+      configFile = args[0];
+    }
+
+    // read configuration
+    if (!readConfig()) {
+      return;
+    }
+
+    // start NetworkTables
+    NetworkTableInstance ntinst = NetworkTableInstance.getDefault();
+    if (server) {
+      System.out.println("Setting up NetworkTables server");
+      ntinst.startServer();
+    } else {
+      System.out.println("Setting up NetworkTables client for team " + team);
+      ntinst.startClientTeam(team);
+    }
+
+    // start cameras
+    for (CameraConfig camera : cameras) {
+      startCamera(camera);
+    }
+
+    // loop forever
+    for (;;) {
+      try {
+        Thread.sleep(10000);
+      } catch (InterruptedException ex) {
+        return;
+      }
+    }
+  }
+}
diff --git a/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp b/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
new file mode 100644
index 0000000000..dbfc7b6fe2
--- /dev/null
+++ b/cameraserver/multiCameraServer/src/main/native/cpp/main.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/settings.gradle b/settings.gradle
index f025441b6e..553adc954a 100644
--- a/settings.gradle
+++ b/settings.gradle
@@ -25,5 +25,6 @@ include 'simulation:halsim_gazebo'
 include 'simulation:lowfi_simulation'
 include 'simulation:halsim_ds_socket'
 include 'cameraserver'
+include 'cameraserver:multiCameraServer'
 include 'myRobot'
 include 'docs'