Only log CameraImage's at 1 Hz
Add an image_decimator application to resend CameraImage messages at 1
Hz.
Change-Id: I12ef64ba7e196ca3d560a7fb3d42b51676176b9f
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/BUILD b/y2022/BUILD
index 488026e..b16067d 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -39,6 +39,7 @@
"//y2022/vision:viewer",
"//y2022/localizer:imu_main",
"//y2022/localizer:localizer_main",
+ "//y2022/vision:image_decimator",
],
data = [
":aos_config",
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 4ca8561..bede03a 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -252,3 +252,15 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+cc_binary(
+ name = "image_decimator",
+ srcs = ["image_decimator.cc"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ "//aos:flatbuffers",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:vision_fbs",
+ ],
+)
diff --git a/y2022/vision/image_decimator.cc b/y2022/vision/image_decimator.cc
new file mode 100644
index 0000000..5fda423
--- /dev/null
+++ b/y2022/vision/image_decimator.cc
@@ -0,0 +1,52 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/flatbuffers.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace frc971::vision {
+// Reads images from /camera and resends them in /camera/decimated at a fixed
+// rate (1 Hz, in this case).
+class ImageDecimator {
+ public:
+ ImageDecimator(aos::EventLoop *event_loop)
+ : slow_image_sender_(
+ event_loop->MakeSender<CameraImage>("/camera/decimated")),
+ image_fetcher_(event_loop->MakeFetcher<CameraImage>("/camera")) {
+ aos::TimerHandler *timer =
+ event_loop->AddTimer(
+ [this]() {
+ if (image_fetcher_.Fetch()) {
+ const aos::FlatbufferSpan<CameraImage> image(
+ {reinterpret_cast<const uint8_t *>(
+ image_fetcher_.context().data),
+ image_fetcher_.context().size});
+ slow_image_sender_.CheckOk(slow_image_sender_.Send(image));
+ }
+ });
+ event_loop->OnRun([event_loop, timer]() {
+ timer->Setup(event_loop->monotonic_now(),
+ std::chrono::milliseconds(1000));
+ });
+ }
+
+ private:
+ aos::Sender<CameraImage> slow_image_sender_;
+ aos::Fetcher<CameraImage> image_fetcher_;
+};
+}
+
+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());
+ frc971::vision::ImageDecimator decimator(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index d6fa4a4..edf52ab 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -322,7 +322,7 @@
"max_size": 200
},
{
- "name": "/pi1/camera",
+ "name": "/pi1/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -338,7 +338,7 @@
]
},
{
- "name": "/pi2/camera",
+ "name": "/pi2/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -354,7 +354,7 @@
]
},
{
- "name": "/pi3/camera",
+ "name": "/pi3/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -370,7 +370,7 @@
]
},
{
- "name": "/pi4/camera",
+ "name": "/pi4/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 7f49e41..27bc251 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -146,6 +146,14 @@
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera/decimated",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 2,
+ "max_size": 620000,
+ "num_senders": 18
+ },
+ {
"name": "/pi{{ NUM }}/camera",
"type": "frc971.vision.calibration.CalibrationData",
"source_node": "pi{{ NUM }}",
@@ -328,6 +336,13 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "image_decimator",
+ "executable_name": "image_decimator",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [