loggerd: broadcast encoder data (#24177)

* encode data try 1

* fix pc build

* low quality compressed streamer with pyav

* nvidia streamer

* timestamp support

* fix latency measurement

* refactor

* camerastream updates

* fix linter

* compressed to vipc, clean

* remove print

* touchups

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: dab978083b7014faa2aef2f43830e87323188578

selfdrive/loggerd/encoder.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include "selfdrive/loggerd/loggerd.h"
 
 class VideoEncoder {
 public:

selfdrive/loggerd/loggerd.cc

@@ -62,12 +62,12 @@ void encoder_thread(LoggerdState *s, const LogCameraInfo &cam_info) {
       LOGD("encoder init %dx%d", buf_info.width, buf_info.height);
 
       // main encoder
-      encoders.push_back(new Encoder(cam_info.filename, buf_info.width, buf_info.height,
+      encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height,
                                      cam_info.fps, cam_info.bitrate, cam_info.is_h265,
                                      cam_info.downscale, cam_info.record));
       // qcamera encoder
       if (cam_info.has_qcamera) {
-        encoders.push_back(new Encoder(qcam_info.filename, qcam_info.frame_width, qcam_info.frame_height,
+        encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, qcam_info.frame_width, qcam_info.frame_height,
                                        qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265, qcam_info.downscale));
       }
     }

selfdrive/loggerd/omx_encoder.cc

@@ -1,6 +1,7 @@
 #pragma clang diagnostic ignored "-Wdeprecated-declarations"
 
 #include "selfdrive/loggerd/omx_encoder.h"
+#include "cereal/messaging/messaging.h"
 
 #include <fcntl.h>
 #include <sys/stat.h>
@@ -153,8 +154,9 @@ static const char* omx_color_fomat_name(uint32_t format) {
 
 
 // ***** encoder functions *****
-OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write) {
+OmxEncoder::OmxEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write) {
   this->filename = filename;
+  this->type = type;
   this->write = write;
   this->width = width;
   this->height = height;
@@ -362,6 +364,32 @@ void OmxEncoder::callback_handler(OmxEncoder *e) {
   }
 }
 
+void OmxEncoder::write_and_broadcast_handler(OmxEncoder *e){
+  bool exit = false;
+  const char *service_name = e->type == DriverCam ? "driverEncodeData" : (e->type == WideRoadCam ? "wideRoadEncodeData" : "roadEncodeData");
+  PubMaster pm({service_name});
+
+  uint32_t idx = 0;
+  while (!exit) {
+    OmxBuffer *out_buf = e->to_write.pop();
+
+    MessageBuilder msg;
+    auto edata = (e->type == DriverCam) ? msg.initEvent(true).initDriverEncodeData() :
+      ((e->type == WideRoadCam) ? msg.initEvent(true).initWideRoadEncodeData() : msg.initEvent(true).initRoadEncodeData());
+    edata.setData(kj::heapArray<capnp::byte>(out_buf->data, out_buf->header.nFilledLen));
+    edata.setTimestampEof(out_buf->header.nTimeStamp);
+    edata.setIdx(idx++);
+    pm.send(service_name, msg);
+
+    OmxEncoder::handle_out_buf(e, out_buf);
+    if (out_buf->header.nFlags & OMX_BUFFERFLAG_EOS) {
+      exit = true;
+    }
+
+    free(out_buf);
+  }
+}
+
 
 void OmxEncoder::write_handler(OmxEncoder *e){
   bool exit = false;
@@ -561,7 +589,7 @@ void OmxEncoder::encoder_open(const char* path) {
 
   // start writer threads
   callback_handler_thread = std::thread(OmxEncoder::callback_handler, this);
-  write_handler_thread = std::thread(OmxEncoder::write_handler, this);
+  write_handler_thread = std::thread(this->remuxing ? OmxEncoder::write_handler : OmxEncoder::write_and_broadcast_handler, this);
 
   this->is_open = true;
   this->counter = 0;

selfdrive/loggerd/omx_encoder.h

@@ -22,7 +22,7 @@ struct OmxBuffer {
 // OmxEncoder, lossey codec using hardware hevc
 class OmxEncoder : public VideoEncoder {
 public:
-  OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write = true);
+  OmxEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write = true);
   ~OmxEncoder();
   int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
                    int in_width, int in_height, uint64_t ts);
@@ -41,6 +41,7 @@ private:
   void wait_for_state(OMX_STATETYPE state);
   static void callback_handler(OmxEncoder *e);
   static void write_handler(OmxEncoder *e);
+  static void write_and_broadcast_handler(OmxEncoder *e);
   static void handle_out_buf(OmxEncoder *e, OmxBuffer *out_buf);
 
   int width, height, fps;
@@ -55,6 +56,7 @@ private:
 
   const char* filename;
   FILE *of = nullptr;
+  CameraType type;
 
   size_t codec_config_len;
   uint8_t *codec_config = NULL;

selfdrive/loggerd/raw_logger.cc

@@ -22,7 +22,7 @@ extern "C" {
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/util.h"
 
-RawLogger::RawLogger(const char* filename, int width, int height, int fps,
+RawLogger::RawLogger(const char* filename, CameraType type, int width, int height, int fps,
                      int bitrate, bool h265, bool downscale, bool write)
   : filename(filename), fps(fps) {
 

selfdrive/loggerd/raw_logger.h

@@ -15,7 +15,7 @@ extern "C" {
 
 class RawLogger : public VideoEncoder {
  public:
-  RawLogger(const char* filename, int width, int height, int fps,
+  RawLogger(const char* filename, CameraType type, int width, int height, int fps,
             int bitrate, bool h265, bool downscale, bool write = true);
   ~RawLogger();
   int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,