Create camera_monitor to restart camera_reader when stuck
Apparently camera_reader can sometimes end up in a state where it stops
sending any images. Create a process that restarts camera_reader when we
stop observing new camera images.
Change-Id: Idd17e174c97776f97eca452e46fa9645645500b1
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/BUILD b/y2023/BUILD
index c3f7c33..6be5cac 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -55,6 +55,7 @@
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
"//y2023/vision:localization_verifier",
+ "//y2023/vision:camera_monitor",
"//y2023/vision:aprilrobotics",
"//aos/events:aos_timing_report_streamer",
"//y2023/localizer:localizer_main",
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index eda123e..ca057fd 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -298,3 +298,25 @@
srcs = ["game_pieces_detector_starter.sh"],
visibility = ["//visibility:public"],
)
+
+cc_library(
+ name = "camera_monitor_lib",
+ srcs = ["camera_monitor_lib.cc"],
+ hdrs = ["camera_monitor_lib.h"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/starter:starter_rpc_lib",
+ "//frc971/vision:vision_fbs",
+ ],
+)
+
+cc_binary(
+ name = "camera_monitor",
+ srcs = ["camera_monitor.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":camera_monitor_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2023/vision/camera_monitor.cc b/y2023/vision/camera_monitor.cc
new file mode 100644
index 0000000..e69fd7c
--- /dev/null
+++ b/y2023/vision/camera_monitor.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/camera_monitor_lib.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ y2023::vision::CameraMonitor monitor(&event_loop);
+
+ event_loop.Run();
+}
diff --git a/y2023/vision/camera_monitor_lib.cc b/y2023/vision/camera_monitor_lib.cc
new file mode 100644
index 0000000..c4ec0dc
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.cc
@@ -0,0 +1,37 @@
+#include "y2023/vision/camera_monitor_lib.h"
+namespace y2023::vision {
+namespace {
+// This needs to include the amount of time that it will take for the
+// camera_reader to start.
+constexpr std::chrono::seconds kImageTimeout{5};
+} // namespace
+CameraMonitor::CameraMonitor(aos::EventLoop *event_loop)
+ : event_loop_(event_loop), starter_(event_loop_) {
+ event_loop_->MakeNoArgWatcher<frc971::vision::CameraImage>(
+ "/camera", [this]() { SetImageTimeout(); });
+ starter_.SetTimeoutHandler([this]() {
+ LOG(WARNING) << "Failed to restart camera_reader when images timed out.";
+ SetImageTimeout();
+ });
+ starter_.SetSuccessHandler([this]() {
+ LOG(INFO) << "Finished restarting camera_reader.";
+ SetImageTimeout();
+ });
+
+ image_timeout_ = event_loop_->AddTimer([this]() {
+ LOG(INFO) << "Restarting camera_reader due to stale images.";
+ starter_.SendCommands({{aos::starter::Command::RESTART,
+ "camera_reader",
+ {event_loop_->node()}}},
+ /*timeout=*/std::chrono::seconds(3));
+ });
+ // If for some reason camera_reader fails to start up at all, we want to
+ // end up restarting things.
+ event_loop_->OnRun([this]() { SetImageTimeout(); });
+}
+
+void CameraMonitor::SetImageTimeout() {
+ image_timeout_->Setup(event_loop_->context().monotonic_event_time +
+ kImageTimeout);
+}
+} // namespace y2023::vision
diff --git a/y2023/vision/camera_monitor_lib.h b/y2023/vision/camera_monitor_lib.h
new file mode 100644
index 0000000..ddb116e
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.h
@@ -0,0 +1,21 @@
+#ifndef Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#define Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#include "aos/events/event_loop.h"
+#include "aos/starter/starter_rpc_lib.h"
+#include "frc971/vision/vision_generated.h"
+namespace y2023::vision {
+// This class provides an application that will restart the camera_reader
+// process whenever images stop flowing for too long. This is to mitigate an
+// issue where sometimes we stop getting camera images.
+class CameraMonitor {
+ public:
+ CameraMonitor(aos::EventLoop *event_loop);
+
+ private:
+ void SetImageTimeout();
+ aos::EventLoop *event_loop_;
+ aos::starter::StarterClient starter_;
+ aos::TimerHandler *image_timeout_;
+};
+} // namespace y2023::vision
+#endif // Y2023_VISION_CAMERA_MONITOR_LIB_H_
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index b0ece47..8c7f245 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -447,6 +447,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
"name": "game_pieces_detector_starter",
"executable_name": "game_pieces_detector_starter.sh",
"autostart": true,
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 46678f3..8167ba0 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -384,6 +384,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
+ },
+ {
"name": "foxglove_websocket",
"user": "pi",
"nodes": [