增加远程、本地查看摄像头功能

2026-08-22 04:42:32 +00:00 · 2026-08-15 19:17:12 +08:00
parent b54ad411c0
commit f3a99f9467
11 changed files with 2346 additions and 289 deletions
@@ -0,0 +1,458 @@
+#include "selfdrive/fp/streamup845.h"
+
+#include <algorithm>
+#include <arpa/inet.h>
+#include <cerrno>
+#include <cstdlib>
+#include <cstring>
+#include <netdb.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "common/swaglog.h"
+#include "third_party/libyuv/include/libyuv.h"
+
+extern "C" {
+#include <libavutil/opt.h>
+}
+
+namespace {
+
+constexpr auto RECONNECT_DELAY = std::chrono::seconds(2);
+
+bool write_all(int fd, const uint8_t *data, size_t size) {
+  while (size > 0) {
+    const ssize_t written = send(fd, data, size, MSG_NOSIGNAL);
+    if (written < 0) {
+      if (errno == EINTR) {
+        continue;
+      }
+      return false;
+    }
+    data += written;
+    size -= static_cast<size_t>(written);
+  }
+  return true;
+}
+
+}  // namespace
+
+AmdH264VaapiEncoder::AmdH264VaapiEncoder(int width, int height, const std::string &camera,
+                                         int fps, int bitrate, const std::string &device_path,
+                                         const std::string &host, int port)
+    : width_(width),
+      height_(height),
+      fps_(fps),
+      bitrate_(bitrate),
+      camera_(camera),
+      host_(host),
+      port_(port),
+      next_reconnect_(std::chrono::steady_clock::now()) {
+  (void)device_path;
+  if (const char *host_env = getenv("H264_WS_HOST"); host_env && host_env[0] != '\0') {
+    host_ = host_env;
+  }
+  if (const char *port_env = getenv("H264_WS_PORT"); port_env && port_env[0] != '\0') {
+    port_ = std::max(1, atoi(port_env));
+  }
+  if (const char *fps_env = getenv("H264_WS_FPS"); fps_env && fps_env[0] != '\0') {
+    fps_ = std::max(1, atoi(fps_env));
+  }
+  if (const char *bitrate_env = getenv("H264_WS_BITRATE"); bitrate_env && bitrate_env[0] != '\0') {
+    bitrate_ = std::max(1, atoi(bitrate_env));
+  }
+
+  gop_size_ = fps_ > 0 ? fps_ * 2 : 40;
+  sw_frame_ = av_frame_alloc();
+  convert_buf_.resize(static_cast<size_t>(width_) * height_ * 3 / 2);
+}
+
+AmdH264VaapiEncoder::~AmdH264VaapiEncoder() {
+  close_socket();
+  close();
+  av_frame_free(&sw_frame_);
+}
+
+bool AmdH264VaapiEncoder::open() {
+  if (opened_) {
+    return true;
+  }
+
+  codec_ = avcodec_find_encoder(AV_CODEC_ID_H264);
+  if (!codec_) {
+    LOGE("FFmpeg H.264 encoder not found");
+    return false;
+  }
+
+  codec_ctx_ = avcodec_alloc_context3(codec_);
+  if (!codec_ctx_) {
+    LOGE("avcodec_alloc_context3 failed");
+    return false;
+  }
+
+  codec_ctx_->width = width_;
+  codec_ctx_->height = height_;
+  codec_ctx_->time_base = AVRational{1, fps_};
+  codec_ctx_->framerate = AVRational{fps_, 1};
+  codec_ctx_->pix_fmt = AV_PIX_FMT_YUV420P;
+  codec_ctx_->bit_rate = bitrate_;
+  codec_ctx_->gop_size = gop_size_;
+  codec_ctx_->max_b_frames = 0;
+  codec_ctx_->color_range = AVCOL_RANGE_MPEG;
+  codec_ctx_->colorspace = AVCOL_SPC_BT709;
+  codec_ctx_->color_primaries = AVCOL_PRI_BT709;
+  codec_ctx_->color_trc = AVCOL_TRC_BT709;
+
+  AVDictionary *opts = nullptr;
+  av_dict_set(&opts, "preset", "ultrafast", 0);
+  av_dict_set(&opts, "tune", "zerolatency", 0);
+  av_dict_set(&opts, "x264-params", "annexb=1:repeat-headers=1", 0);
+  const int ret = avcodec_open2(codec_ctx_, codec_, &opts);
+  av_dict_free(&opts);
+  if (ret < 0) {
+    LOGE("avcodec_open2(H.264) failed: %d", ret);
+    release();
+    return false;
+  }
+
+  opened_ = true;
+  frame_idx_ = 0;
+  return true;
+}
+
+void AmdH264VaapiEncoder::release() {
+  if (codec_ctx_) {
+    avcodec_free_context(&codec_ctx_);
+  }
+  opened_ = false;
+}
+
+void AmdH264VaapiEncoder::close() {
+  if (!opened_ && !codec_ctx_) {
+    return;
+  }
+
+  if (codec_ctx_) {
+    avcodec_send_frame(codec_ctx_, nullptr);
+    drain_packets(nullptr);
+  }
+  release();
+}
+
+bool AmdH264VaapiEncoder::drain_packets(std::vector<uint8_t> *out) {
+  AVPacket *pkt = av_packet_alloc();
+  if (!pkt) {
+    LOGE("av_packet_alloc failed");
+    return false;
+  }
+
+  while (true) {
+    const int ret = avcodec_receive_packet(codec_ctx_, pkt);
+    if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
+      av_packet_free(&pkt);
+      return true;
+    }
+    if (ret < 0) {
+      LOGE("avcodec_receive_packet failed: %d", ret);
+      av_packet_free(&pkt);
+      return false;
+    }
+    if (out) {
+      out->insert(out->end(), pkt->data, pkt->data + pkt->size);
+    }
+    av_packet_unref(pkt);
+  }
+}
+
+bool AmdH264VaapiEncoder::ensure_connected() {
+  if (socket_fd_ >= 0) {
+    return true;
+  }
+
+  const auto now = std::chrono::steady_clock::now();
+  if (now < next_reconnect_) {
+    return false;
+  }
+  next_reconnect_ = now + RECONNECT_DELAY;
+
+  addrinfo hints = {};
+  hints.ai_family = AF_UNSPEC;
+  hints.ai_socktype = SOCK_STREAM;
+
+  addrinfo *result = nullptr;
+  const std::string port = std::to_string(port_);
+  if (getaddrinfo(host_.c_str(), port.c_str(), &hints, &result) != 0) {
+    return false;
+  }
+
+  for (addrinfo *rp = result; rp != nullptr; rp = rp->ai_next) {
+    const int fd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
+    if (fd < 0) {
+      continue;
+    }
+    if (connect(fd, rp->ai_addr, rp->ai_addrlen) == 0) {
+      socket_fd_ = fd;
+      break;
+    }
+    ::close(fd);
+  }
+  freeaddrinfo(result);
+
+  if (socket_fd_ < 0) {
+    return false;
+  }
+  if (!send_handshake() || !send_camera_register()) {
+    close_socket();
+    return false;
+  }
+
+  has_viewer_ = false;
+  restart_encoder_ = true;
+  return true;
+}
+
+bool AmdH264VaapiEncoder::send_handshake() {
+  const std::string path = "/ingest?camera=" + camera_;
+  const std::string request =
+      "GET " + path + " HTTP/1.1\r\n"
+      "Host: " + host_ + ":" + std::to_string(port_) + "\r\n"
+      "Upgrade: websocket\r\n"
+      "Connection: Upgrade\r\n"
+      "Sec-WebSocket-Key: ZHJhZ29ucGlsb3QtaDI2NA==\r\n"
+      "Sec-WebSocket-Version: 13\r\n"
+      "\r\n";
+  if (!write_all(socket_fd_, reinterpret_cast<const uint8_t *>(request.data()), request.size())) {
+    return false;
+  }
+
+  std::string response;
+  char buffer[512];
+  while (response.find("\r\n\r\n") == std::string::npos && response.size() < 4096) {
+    const ssize_t size = recv(socket_fd_, buffer, sizeof(buffer), 0);
+    if (size <= 0) {
+      return false;
+    }
+    response.append(buffer, static_cast<size_t>(size));
+  }
+  return response.find(" 101 ") != std::string::npos;
+}
+
+bool AmdH264VaapiEncoder::send_camera_register() {
+  return send_packet(0, reinterpret_cast<const uint8_t *>(camera_.data()), camera_.size());
+}
+
+bool AmdH264VaapiEncoder::send_packet(uint8_t cmd, const uint8_t *data, size_t size) {
+  if (size > 0xffffff) {
+    LOGE("H.264 websocket payload too large: %zu", size);
+    return false;
+  }
+
+  std::vector<uint8_t> packet;
+  packet.reserve(size + 4);
+  packet.push_back(cmd);
+  packet.push_back(static_cast<uint8_t>((size >> 16) & 0xff));
+  packet.push_back(static_cast<uint8_t>((size >> 8) & 0xff));
+  packet.push_back(static_cast<uint8_t>(size & 0xff));
+  packet.insert(packet.end(), data, data + size);
+  return send_ws_binary(packet.data(), packet.size());
+}
+
+bool AmdH264VaapiEncoder::send_ws_binary(const uint8_t *data, size_t size) {
+  std::vector<uint8_t> header = {0x82};
+  if (size <= 125) {
+    header.push_back(static_cast<uint8_t>(size));
+  } else if (size <= 0xffff) {
+    header.push_back(126);
+    header.push_back(static_cast<uint8_t>((size >> 8) & 0xff));
+    header.push_back(static_cast<uint8_t>(size & 0xff));
+  } else {
+    header.push_back(127);
+    for (int shift = 56; shift >= 0; shift -= 8) {
+      header.push_back(static_cast<uint8_t>((static_cast<uint64_t>(size) >> shift) & 0xff));
+    }
+  }
+  return write_all(socket_fd_, header.data(), header.size()) && write_all(socket_fd_, data, size);
+}
+
+void AmdH264VaapiEncoder::read_control_messages() {
+  if (socket_fd_ < 0) {
+    return;
+  }
+
+  while (true) {
+    uint8_t data[4096];
+    const ssize_t size = recv(socket_fd_, data, sizeof(data), MSG_DONTWAIT);
+    if (size == 0) {
+      close_socket();
+      return;
+    }
+    if (size < 0) {
+      if (errno == EAGAIN || errno == EWOULDBLOCK || errno == EINTR) {
+        break;
+      }
+      close_socket();
+      return;
+    }
+    rx_buffer_.insert(rx_buffer_.end(), data, data + size);
+  }
+
+  size_t offset = 0;
+  while (rx_buffer_.size() - offset >= 2) {
+    const uint8_t *header = rx_buffer_.data() + offset;
+    const uint8_t opcode = header[0] & 0x0f;
+    const bool masked = (header[1] & 0x80) != 0;
+    uint64_t payload_size = header[1] & 0x7f;
+    size_t header_size = 2;
+
+    if (payload_size == 126) {
+      if (rx_buffer_.size() - offset < 4) break;
+      payload_size = (static_cast<uint64_t>(header[2]) << 8) | header[3];
+      header_size = 4;
+    } else if (payload_size == 127) {
+      if (rx_buffer_.size() - offset < 10) break;
+      payload_size = 0;
+      for (size_t index = 2; index < 10; ++index) {
+        payload_size = (payload_size << 8) | header[index];
+      }
+      header_size = 10;
+    }
+
+    if (payload_size > 1024 * 1024) {
+      close_socket();
+      return;
+    }
+
+    const size_t mask_size = masked ? 4 : 0;
+    const uint64_t frame_size = header_size + mask_size + payload_size;
+    if (frame_size > rx_buffer_.size() - offset) break;
+
+    const uint8_t *mask = masked ? header + header_size : nullptr;
+    const uint8_t *payload_data = header + header_size + mask_size;
+    std::vector<uint8_t> payload(payload_data, payload_data + payload_size);
+    if (masked) {
+      for (size_t index = 0; index < payload.size(); ++index) {
+        payload[index] ^= mask[index % 4];
+      }
+    }
+    offset += static_cast<size_t>(frame_size);
+
+    if (opcode == 0x8) {
+      close_socket();
+      return;
+    }
+    if ((opcode == 0x1 || opcode == 0x2) && payload.size() >= 5 && payload[0] == 2) {
+      const bool has_viewer = payload[4] == '1';
+      if (has_viewer && !has_viewer_) {
+        restart_encoder_ = true;
+      }
+      has_viewer_ = has_viewer;
+    } else if ((opcode == 0x1 || opcode == 0x2) && payload.size() >= 4 && payload[0] == 3) {
+      restart_encoder_ = true;
+    }
+  }
+
+  if (offset > 0) {
+    rx_buffer_.erase(rx_buffer_.begin(), rx_buffer_.begin() + offset);
+  }
+}
+
+void AmdH264VaapiEncoder::close_socket() {
+  if (socket_fd_ >= 0) {
+    ::close(socket_fd_);
+    socket_fd_ = -1;
+  }
+  has_viewer_ = false;
+  restart_encoder_ = true;
+  rx_buffer_.clear();
+}
+
+bool AmdH264VaapiEncoder::fps_limited() const {
+  if (fps_ <= 0 || last_sent_.time_since_epoch().count() == 0) {
+    return false;
+  }
+  const auto interval = std::chrono::microseconds(1000000 / fps_);
+  return std::chrono::steady_clock::now() - last_sent_ < interval;
+}
+
+bool AmdH264VaapiEncoder::encode_frame(const VisionBuf *buf) {
+  if (!buf) {
+    return false;
+  }
+
+  if (!ensure_connected()) {
+    return true;
+  }
+  read_control_messages();
+  if (!has_viewer_ || fps_limited()) {
+    return true;
+  }
+
+  bool force_keyframe = false;
+  if (restart_encoder_) {
+    close();
+    force_keyframe = true;
+    restart_encoder_ = false;
+  }
+
+  if (!opened_ && !open()) {
+    restart_encoder_ = true;
+    return false;
+  }
+  if (buf->width != static_cast<size_t>(width_) || buf->height != static_cast<size_t>(height_)) {
+    LOGE("input size mismatch: got %zux%zu expect %dx%d", buf->width, buf->height, width_, height_);
+    return false;
+  }
+
+  uint8_t *y = convert_buf_.data();
+  uint8_t *u = y + static_cast<size_t>(width_) * height_;
+  uint8_t *v = u + static_cast<size_t>(width_ / 2) * (height_ / 2);
+  const int convert_ret = libyuv::NV12ToI420(
+      buf->y, static_cast<int>(buf->stride),
+      buf->uv, static_cast<int>(buf->stride),
+      y, width_,
+      u, width_ / 2,
+      v, width_ / 2,
+      width_, height_);
+  if (convert_ret != 0) {
+    LOGE("NV12ToI420 failed: %d", convert_ret);
+    return false;
+  }
+
+  av_frame_unref(sw_frame_);
+  sw_frame_->format = AV_PIX_FMT_YUV420P;
+  sw_frame_->width = width_;
+  sw_frame_->height = height_;
+  sw_frame_->data[0] = y;
+  sw_frame_->data[1] = u;
+  sw_frame_->data[2] = v;
+  sw_frame_->linesize[0] = width_;
+  sw_frame_->linesize[1] = width_ / 2;
+  sw_frame_->linesize[2] = width_ / 2;
+  sw_frame_->pts = frame_idx_;
+  sw_frame_->pict_type =
+      (force_keyframe || (gop_size_ > 0 && frame_idx_ % gop_size_ == 0))
+          ? AV_PICTURE_TYPE_I
+          : AV_PICTURE_TYPE_NONE;
+
+  const int ret = avcodec_send_frame(codec_ctx_, sw_frame_);
+  if (ret < 0) {
+    LOGE("avcodec_send_frame failed: %d", ret);
+    return false;
+  }
+
+  std::vector<uint8_t> encoded;
+  if (!drain_packets(&encoded)) {
+    return false;
+  }
+
+  ++frame_idx_;
+  if (encoded.empty()) {
+    return true;
+  }
+  if (!send_packet(1, encoded.data(), encoded.size())) {
+    close_socket();
+    return false;
+  }
+  last_sent_ = std::chrono::steady_clock::now();
+  return true;
+}