burrow-pi-img/deps/examples/cpp-multiCameraServer/main.cpp

222 lines
6.0 KiB
C++
Raw Normal View History

/*----------------------------------------------------------------------------*/
/* 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 <thread>
#include <vector>
#include <networktables/NetworkTableInstance.h>
#include <vision/VisionPipeline.h>
#include <vision/VisionRunner.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>
}
]
}
]
}
*/
static const char* configFile = "/boot/frc.json";
namespace {
unsigned int team;
bool server = false;
struct CameraConfig {
std::string name;
std::string path;
wpi::json config;
};
std::vector<CameraConfig> cameraConfigs;
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;
cameraConfigs.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;
}
cs::UsbCamera 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);
camera.SetConnectionStrategy(cs::VideoSource::kConnectionKeepOpen);
return camera;
}
// example pipeline
class MyPipeline : public frc::VisionPipeline {
public:
int val = 0;
void Process(cv::Mat& mat) override {
++val;
}
};
} // 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
std::vector<cs::VideoSource> cameras;
for (auto&& cameraConfig : cameraConfigs)
cameras.emplace_back(StartCamera(cameraConfig));
// start image processing on camera 0 if present
if (cameras.size() >= 1) {
std::thread([&] {
frc::VisionRunner<MyPipeline> runner(cameras[0], new MyPipeline(),
[&](MyPipeline &pipeline) {
// do something with pipeline results
});
/* something like this for GRIP:
frc::VisionRunner<MyPipeline> runner(cameras[0], new grip::GripPipeline(),
[&](grip::GripPipeline& pipeline) {
...
});
*/
runner.RunForever();
}).detach();
}
// loop forever
for (;;) std::this_thread::sleep_for(std::chrono::seconds(10));
}