diff --git a/README.md b/README.md index 0005d68..b8c4b59 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ export ATHENA_HOST='ws://laolang.duckdns.org:7899' export API_HOST='http://laolang.duckdns.org:7898' -export MAPBOX_TOKEN='pk.eyJ1Ijoiam5ld2IiLCJhIjoiY2xxNW8zZXprMGw1ZzJwbzZneHd2NHljbSJ9.gV7VPRfbXFetD-1OVF0XZg' +export MAPBOX_TOKEN='<自己注册>' cd /data/openpilot exec ./launch_openpilot.sh @@ -51,7 +51,108 @@ exec ./launch_openpilot.sh #### 5. 暴露端口和运行docker sudo docker run -p 7899:7899 -p 7898:7898 -p 7888:7888 -p 1201:1201 -p 5888:5888 opsvr:v1 -#### 6.Star me. Thank you.如果觉得很复杂，可以直接看第二步，使用我的服务器。 +#### 6. 在 openpilot 中启用 H.264 实时画面 + +(1) 确保准备好以下文件，并复制到设备的 +`/data/openpilot/selfdrive/fp/`： + +```text +SConscript +main.cc +streamup845.cc +streamup845.h +viewer.html +websocket_server.py +``` + +其中 `main.cc` 是 `h264_streamer` 的程序入口，缺少它将无法生成 +`selfdrive/fp/h264_streamer`。当前发布包如果没有该文件，需要从对应的 +openpilot/dragonpilot H.264 实现中补齐。 + +骁龙 845（C3/C3X，构建架构为 `larch64`）使用 +`streamup845.cc`。其他 Linux PC/AMD 平台需要同时提供 +`streamup.cc` 和 `streamup.h`。 + +(2) 在 `/data/openpilot/selfdrive/SConscript` 中增加： + +```python +SConscript(['fp/SConscript']) +``` + +(3) 在 `/data/openpilot/system/manager/process_config.py` 的进程列表中增加： + +```python +NativeProcess("h264_streamer", "selfdrive/fp", ["./h264_streamer"], always_run), +PythonProcess("websocket_server", "selfdrive.fp.websocket_server", always_run), +``` + +(4) 在 `/data/continue.sh` 中配置远端 WebSocket 服务。`ATHENA_HOST` +必须指向服务端 `config.txt` 的 `ws_bind` 公网地址： + +```bash +export ATHENA_HOST='ws://your-domain.example:7899' + +# 可选配置，以下为默认值 +export H264_WS_PORT='8089' +export H264_WS_FPS='20' +export H264_WS_BITRATE='2000000' +export H264_REMOTE_MAX_LATENCY='1.5' +``` + +通常不要设置 `H264_WS_HOST`。它同时用于设备本地 +`websocket_server.py` 的监听地址，以及 `h264_streamer` 连接本地服务。 +不设置时，Python 服务监听 `0.0.0.0:8089`，编码器连接 +`127.0.0.1:8089`。不要将它设置为公网服务器地址，也不要显式设置为 +`0.0.0.0`。 + +(5) 重新编译并确认生成可执行文件： + +```bash +cd /data/openpilot +scons -j$(nproc) selfdrive/fp/h264_streamer +ls -l selfdrive/fp/h264_streamer +``` + +如果当前 openpilot 版本不支持指定单个目标，可以直接运行： + +```bash +scons -j$(nproc) +``` + +(6) 服务端需要满足以下条件： + +- 使用支持 H.264 实时画面的新版 `opserver`。 +- `config.txt` 中 `athena_host`/`ws_bind` 的公网端口可从设备和浏览器访问。 +- 防火墙、Docker 和路由器同时放行 `http_bind` 与 `ws_bind`，例如 TCP + `7898` 和 TCP `7899`。 +- 网页必须连接 `ws_bind` 对应的 WebSocket 端口。如果网页从 + `http://domain:7898` 打开，而脚本使用 `location.host`，它会错误连接 + `7898`。应通过反向代理把 H.264 WebSocket 请求转发到 `7899`，或者在 + 网页中明确使用 `ws_bind` 的公网地址。 +- HTTPS 页面必须使用 `wss://`，不能连接明文 `ws://`；推荐使用 Nginx、 + Caddy 等为 HTTP 和 WebSocket 提供同一 HTTPS 域名。 +- 浏览器需要支持 WebCodecs H.264，建议使用新版 Chrome 或 Edge。 + +(7) 重启后检查进程和端口： + +```bash +sudo reboot + +# 重连 SSH 后执行 +pgrep -af 'h264_streamer|websocket_server' +ss -lntp | grep 8089 +``` + +局域网测试： + +```text +http://<设备IP>:8089/viewer.html +``` + +广域网测试：打开服务端网页，输入正确的 Dongle ID，再选择 Road Cam 或 +Wide Cam。必须先确保设备的 `DongleId` 不为空，并且与网页输入值一致。 + +#### 7.Star me. Thank you.如果觉得很复杂，可以直接看第二步，使用我的服务器。 # openpilot-server for english diff --git a/openpilot/selfdrive/fp/SConscript b/openpilot/selfdrive/fp/SConscript new file mode 100644 index 0000000..7395a8b --- /dev/null +++ b/openpilot/selfdrive/fp/SConscript @@ -0,0 +1,12 @@ +Import('env', 'arch', 'common', 'messaging', 'visionipc') + +if arch != "Darwin": + streamup_src = 'streamup845.cc' if arch == "larch64" else 'streamup.cc' + libs = [visionipc, messaging, common, + 'avcodec', 'avutil', + 'OpenCL', + 'pthread'] + if arch == "larch64": + libs += ['yuv'] + env.Library('fp', [streamup_src], LIBS=libs) + env.Program('h264_streamer', ['main.cc', streamup_src], LIBS=libs) diff --git a/openpilot/selfdrive/fp/main.cc b/openpilot/selfdrive/fp/main.cc new file mode 100644 index 0000000..13fc98d --- /dev/null +++ b/openpilot/selfdrive/fp/main.cc @@ -0,0 +1,90 @@ +#include +#include +#include +#include + +#include "common/util.h" +#include "msgq/visionipc/visionipc_client.h" +#ifdef QCOM2 +#include "selfdrive/fp/streamup845.h" +#else +#include "selfdrive/fp/streamup.h" +#endif + +namespace { + +constexpr int DEFAULT_FPS = 20; +constexpr int DEFAULT_BITRATE = 2'000'000; + +struct StreamConfig { + VisionStreamType type; + const char *camera; + const char *thread_name; +}; + +void stream_camera(const StreamConfig config, ExitHandler *do_exit) { + util::set_thread_name(config.thread_name); + + while (!*do_exit) { + VisionIpcClient client("camerad", config.type, true); + if (!client.connect(false)) { + util::sleep_for(200); + continue; + } + + const VisionBuf &info = client.buffers[0]; + if (info.width == 0 || info.height == 0) { + fprintf(stderr, "%s invalid VisionBuf size\n", config.camera); + util::sleep_for(1000); + continue; + } + + fprintf(stderr, "%s H.264 streamer connected: %zux%zu\n", + config.camera, info.width, info.height); + + AmdH264VaapiEncoder encoder( + static_cast(info.width), + static_cast(info.height), + config.camera, + DEFAULT_FPS, + DEFAULT_BITRATE, + ""); + + while (!*do_exit && client.is_connected()) { + VisionIpcBufExtra extra = {}; + VisionBuf *buf = client.recv(&extra, 1000); + if (!buf) { + continue; + } + if (buf->get_frame_id() != extra.frame_id) { + continue; + } + + if (!encoder.encode_frame(buf)) { + util::sleep_for(50); + } + } + + fprintf(stderr, "%s VisionIPC disconnected, reconnecting\n", config.camera); + } +} + +} // namespace + +int main() { + ExitHandler do_exit; + const std::vector streams = { + {VISION_STREAM_ROAD, "roadCameraState", "h264_road"}, + {VISION_STREAM_WIDE_ROAD, "wideRoadCameraState", "h264_wide"}, + }; + + std::vector threads; + threads.reserve(streams.size()); + for (const StreamConfig &config : streams) { + threads.emplace_back(stream_camera, config, &do_exit); + } + for (std::thread &thread : threads) { + thread.join(); + } + return 0; +} diff --git a/openpilot/selfdrive/fp/streamup845.cc b/openpilot/selfdrive/fp/streamup845.cc new file mode 100644 index 0000000..46d79a2 --- /dev/null +++ b/openpilot/selfdrive/fp/streamup845.cc @@ -0,0 +1,458 @@ +#include "selfdrive/fp/streamup845.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/swaglog.h" +#include "third_party/libyuv/include/libyuv.h" + +extern "C" { +#include +} + +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(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(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 *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(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)); + } + return response.find(" 101 ") != std::string::npos; +} + +bool AmdH264VaapiEncoder::send_camera_register() { + return send_packet(0, reinterpret_cast(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 packet; + packet.reserve(size + 4); + packet.push_back(cmd); + packet.push_back(static_cast((size >> 16) & 0xff)); + packet.push_back(static_cast((size >> 8) & 0xff)); + packet.push_back(static_cast(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 header = {0x82}; + if (size <= 125) { + header.push_back(static_cast(size)); + } else if (size <= 0xffff) { + header.push_back(126); + header.push_back(static_cast((size >> 8) & 0xff)); + header.push_back(static_cast(size & 0xff)); + } else { + header.push_back(127); + for (int shift = 56; shift >= 0; shift -= 8) { + header.push_back(static_cast((static_cast(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(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 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(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(width_) || buf->height != static_cast(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(width_) * height_; + uint8_t *v = u + static_cast(width_ / 2) * (height_ / 2); + const int convert_ret = libyuv::NV12ToI420( + buf->y, static_cast(buf->stride), + buf->uv, static_cast(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 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; +} diff --git a/openpilot/selfdrive/fp/streamup845.h b/openpilot/selfdrive/fp/streamup845.h new file mode 100644 index 0000000..bf236b8 --- /dev/null +++ b/openpilot/selfdrive/fp/streamup845.h @@ -0,0 +1,68 @@ +#pragma once + +// Snapdragon 845 H.264 encoding through FFmpeg. +// Input NV12 frames are converted to I420 with libyuv before encoding. + +#include +#include +#include +#include + +#include "msgq/visionipc/visionbuf.h" + +extern "C" { +#include +#include +} + +class AmdH264VaapiEncoder { +public: + AmdH264VaapiEncoder(int width, int height, const std::string &camera, + int fps = 20, + int bitrate = 5'000'000, + const std::string &device_path = "", + const std::string &host = "127.0.0.1", + int port = 8089); + ~AmdH264VaapiEncoder(); + + bool open(); + void close(); + bool encode_frame(const VisionBuf *buf); + + bool is_open() const { return opened_; } + +private: + bool drain_packets(std::vector *out); + bool ensure_connected(); + bool send_handshake(); + bool send_camera_register(); + bool send_packet(uint8_t cmd, const uint8_t *data, size_t size); + bool send_ws_binary(const uint8_t *data, size_t size); + void read_control_messages(); + void close_socket(); + bool fps_limited() const; + void release(); + + int width_ = 0; + int height_ = 0; + int fps_ = 0; + int bitrate_ = 0; + std::string camera_; + std::string host_; + int port_ = 8089; + + AVCodecContext *codec_ctx_ = nullptr; + AVFrame *sw_frame_ = nullptr; + const AVCodec *codec_ = nullptr; + std::vector convert_buf_; + + bool opened_ = false; + int socket_fd_ = -1; + bool has_viewer_ = false; + bool restart_encoder_ = true; + int64_t frame_idx_ = 0; + int gop_size_ = 0; + std::vector rx_buffer_; + std::chrono::steady_clock::time_point last_sent_; + std::chrono::steady_clock::time_point next_reconnect_; +}; diff --git a/openpilot/selfdrive/fp/viewer.html b/openpilot/selfdrive/fp/viewer.html new file mode 100644 index 0000000..79bcff1 --- /dev/null +++ b/openpilot/selfdrive/fp/viewer.html @@ -0,0 +1,358 @@ + + + + + + OpenPilot H.264 Preview + + + +
+
+

H.264 Camera Preview

+
连接中
+
+ +
+ +
等待摄像头推送画面...
+
+
+ 端口 8089 / WebSocket: /stream?camera=... + 0 frames +
+
+ + + + diff --git a/openpilot/selfdrive/fp/websocket_server.py b/openpilot/selfdrive/fp/websocket_server.py new file mode 100644 index 0000000..7c554e4 --- /dev/null +++ b/openpilot/selfdrive/fp/websocket_server.py @@ -0,0 +1,434 @@ +#!/usr/bin/env python3 +import asyncio +import base64 +from collections import deque +import hashlib +import os +import struct +import time +from pathlib import Path +from urllib.parse import parse_qs, urlsplit + +from openpilot.common.params import Params + +HOST = os.getenv("H264_WS_HOST", "0.0.0.0") +PORT = int(os.getenv("H264_WS_PORT", "8089")) +PUSH_FPS = float(os.getenv("H264_WS_FPS", "20")) +ATHENA_HOST = os.getenv("ATHENA_HOST", "") +REMOTE_MAX_LATENCY = float(os.getenv("H264_REMOTE_MAX_LATENCY", "1.5")) +HTML_PATH = Path(__file__).with_name("viewer.html") +WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + + +def get_dongle_id(): + return os.getenv("DONGLE_ID") or Params().get("DongleId") or "" + + +class MjpegWebsocketServer: + def __init__(self, push_fps): + self.push_fps = push_fps + self.viewers = {} + self.ingests = {} + self.remote_viewers = {} + self.remote_clients = {} + self.dongle_id = get_dongle_id() + + async def handle_client(self, reader, writer): + try: + request = await self.read_http_request(reader) + if not request: + return + method, path, headers = request + if headers.get("upgrade", "").lower() == "websocket": + await self.handle_websocket(path, headers, reader, writer) + else: + await self.handle_http(method, path, writer) + finally: + writer.close() + await writer.wait_closed() + + async def read_http_request(self, reader): + data = await reader.readuntil(b"\r\n\r\n") + lines = data.decode("iso-8859-1").split("\r\n") + parts = lines[0].split() + if len(parts) < 2: + return None + headers = {} + for line in lines[1:]: + if ":" in line: + key, value = line.split(":", 1) + headers[key.strip().lower()] = value.strip() + return parts[0], parts[1], headers + + async def handle_http(self, method, path, writer): + route = urlsplit(path).path + if method != "GET" or route not in ("/", "/viewer.html"): + await self.send_http(writer, "404 Not Found", b"not found", "text/plain") + return + body = HTML_PATH.read_bytes() + await self.send_http(writer, "200 OK", body, "text/html; charset=utf-8") + + async def send_http(self, writer, status, body, content_type): + writer.write( + f"HTTP/1.1 {status}\r\n" + f"Content-Type: {content_type}\r\n" + f"Content-Length: {len(body)}\r\n" + "Connection: close\r\n\r\n".encode("ascii") + body + ) + await writer.drain() + + async def handle_websocket(self, path, headers, reader, writer): + key = headers.get("sec-websocket-key") + url = urlsplit(path) + route = url.path + camera = parse_qs(url.query).get("camera", ["roadCameraState"])[0] + if not key or route not in ("/stream", "/ingest"): + writer.write(b"HTTP/1.1 400 Bad Request\r\nConnection: close\r\n\r\n") + await writer.drain() + return + + accept = base64.b64encode(hashlib.sha1((key + WS_GUID).encode("ascii")).digest()).decode("ascii") + writer.write( + "HTTP/1.1 101 Switching Protocols\r\n" + "Upgrade: websocket\r\n" + "Connection: Upgrade\r\n" + f"Sec-WebSocket-Accept: {accept}\r\n\r\n".encode("ascii") + ) + await writer.drain() + + if route == "/stream": + await self.viewer_loop(camera, reader, writer) + else: + await self.ingest_loop(camera, reader, writer) + + async def viewer_loop(self, camera, reader, writer): + self.viewers.setdefault(camera, set()).add(writer) + await self.notify_ingests(camera) + await self.request_keyframe(camera) + try: + while True: + opcode, _ = await self.read_ws_frame(reader) + if opcode == 0x8: + break + finally: + self.viewers.get(camera, set()).discard(writer) + await self.notify_ingests(camera) + + async def ingest_loop(self, camera, reader, writer): + self.ingests.setdefault(camera, set()).add(writer) + await self.notify_ingests(camera) + try: + while True: + opcode, payload = await self.read_ws_frame(reader) + if opcode == 0x8: + break + if opcode == 0x2: + cmd, body = self.parse_ingest_packet(payload) + if cmd == 0: + old_camera = camera + camera = body.decode("utf-8", errors="ignore") or camera + if old_camera != camera: + self.ingests.get(old_camera, set()).discard(writer) + await self.notify_ingests(old_camera) + self.ingests.setdefault(camera, set()).add(writer) + await self.notify_ingests(camera) + elif cmd == 1: + await self.broadcast_frame(camera, body) + finally: + self.ingests.get(camera, set()).discard(writer) + + async def notify_ingests(self, camera): + has_viewer = bool(self.viewers.get(camera)) or self.remote_viewers.get(camera, False) + message = self.make_ingest_packet(2, b"1" if has_viewer else b"0") + stale = [] + for writer in self.ingests.get(camera, set()): + try: + await self.write_ws_frame(writer, message, opcode=0x2) + except (ConnectionError, OSError): + stale.append(writer) + for writer in stale: + self.ingests.get(camera, set()).discard(writer) + + async def request_keyframe(self, camera): + message = self.make_ingest_packet(3, b"") + stale = [] + for writer in self.ingests.get(camera, set()): + try: + await self.write_ws_frame(writer, message, opcode=0x2) + except (ConnectionError, OSError): + stale.append(writer) + for writer in stale: + self.ingests.get(camera, set()).discard(writer) + + def parse_ingest_packet(self, payload): + if len(payload) < 4: + return None, b"" + cmd = payload[0] + length = (payload[1] << 16) | (payload[2] << 8) | payload[3] + if 4 + length > len(payload): + return None, b"" + return cmd, payload[4:4 + length] + + def make_ingest_packet(self, cmd, payload): + if len(payload) > 0xffffff: + raise ValueError("payload too large") + return bytes((cmd, (len(payload) >> 16) & 0xff, (len(payload) >> 8) & 0xff, len(payload) & 0xff)) + payload + + @staticmethod + def is_h264_keyframe(payload): + index = 0 + while index + 4 <= len(payload): + if payload[index:index + 4] == b"\x00\x00\x00\x01": + nal_start = index + 4 + index = nal_start + elif payload[index:index + 3] == b"\x00\x00\x01": + nal_start = index + 3 + index = nal_start + else: + index += 1 + continue + if nal_start < len(payload) and payload[nal_start] & 0x1f == 5: + return True + return False + + async def broadcast_frame(self, camera, payload): + viewers = self.viewers.get(camera, set()) + remote_client = await self.ensure_remote_client(camera) + if not viewers and not self.remote_viewers.get(camera, False): + return + + stale = [] + for writer in viewers: + try: + await self.write_ws_frame(writer, payload, opcode=0x2) + except (ConnectionError, OSError): + stale.append(writer) + for writer in stale: + viewers.discard(writer) + if stale: + await self.notify_ingests(camera) + if remote_client and self.remote_viewers.get(camera, False): + await remote_client.send_h264(payload) + + async def read_ws_frame(self, reader): + header = await reader.readexactly(2) + opcode = header[0] & 0x0f + masked = (header[1] & 0x80) != 0 + length = header[1] & 0x7f + if length == 126: + length = struct.unpack("!H", await reader.readexactly(2))[0] + elif length == 127: + length = struct.unpack("!Q", await reader.readexactly(8))[0] + + mask = await reader.readexactly(4) if masked else b"" + payload = await reader.readexactly(length) if length else b"" + if masked: + payload = bytes(byte ^ mask[index % 4] for index, byte in enumerate(payload)) + return opcode, payload + + async def write_ws_frame(self, writer, payload, opcode=0x2): + header = bytearray([0x80 | opcode]) + length = len(payload) + if length <= 125: + header.append(length) + elif length <= 0xffff: + header.extend((126, *struct.pack("!H", length))) + else: + header.extend((127, *struct.pack("!Q", length))) + writer.write(bytes(header) + payload) + await writer.drain() + + async def ensure_remote_client(self, camera): + if not ATHENA_HOST: + return None + client = self.remote_clients.get(camera) + if client: + return client + client = RemoteH264Client(ATHENA_HOST, self.dongle_id, camera, self) + self.remote_clients[camera] = client + asyncio.create_task(client.run()) + return client + + async def start_remote_clients(self): + if not ATHENA_HOST: + return + for camera in ("roadCameraState", "wideRoadCameraState"): + await self.ensure_remote_client(camera) + + +class RemoteH264Client: + def __init__(self, athena_host, dongle_id, camera, server): + self.athena_host = athena_host + self.dongle_id = dongle_id + self.camera = camera + self.server = server + self.reader = None + self.writer = None + self.connected = False + self.write_lock = asyncio.Lock() + self.pending_frames = deque() + self.pending_event = asyncio.Event() + self.waiting_for_keyframe = True + + async def run(self): + while True: + try: + await self.connect() + sender_task = asyncio.create_task(self.send_loop()) + reader_task = asyncio.create_task(self.read_loop()) + try: + done, pending = await asyncio.wait( + (reader_task, sender_task), + return_when=asyncio.FIRST_COMPLETED, + ) + for task in pending: + task.cancel() + await asyncio.gather(*pending, return_exceptions=True) + for task in done: + task.result() + finally: + sender_task.cancel() + reader_task.cancel() + except (ConnectionError, OSError, asyncio.IncompleteReadError, asyncio.TimeoutError): + pass + finally: + self.connected = False + self.waiting_for_keyframe = True + self.clear_pending_frames() + self.server.remote_viewers[self.camera] = False + await self.server.notify_ingests(self.camera) + if self.writer: + self.writer.close() + await self.writer.wait_closed() + await asyncio.sleep(2) + + async def connect(self): + url = self.normalize_url() + parsed = urlsplit(url) + port = parsed.port or (443 if parsed.scheme == "wss" else 80) + ssl_enabled = parsed.scheme == "wss" + self.reader, self.writer = await asyncio.open_connection(parsed.hostname, port, ssl=ssl_enabled) + base_path = (parsed.path or "").rstrip("/") + path = f"{base_path}/h264/ingest?dongle_id={self.dongle_id}&camera={self.camera}" + key = base64.b64encode(os.urandom(16)).decode("ascii") + request = ( + f"GET {path} HTTP/1.1\r\n" + f"Host: {parsed.netloc}\r\n" + "Upgrade: websocket\r\n" + "Connection: Upgrade\r\n" + f"Sec-WebSocket-Key: {key}\r\n" + "Sec-WebSocket-Version: 13\r\n" + "\r\n" + ) + self.writer.write(request.encode("ascii")) + await self.writer.drain() + response = await self.reader.readuntil(b"\r\n\r\n") + if b" 101 " not in response: + raise ConnectionError("remote websocket upgrade failed") + self.connected = True + await self.send_packet(0, self.camera.encode("utf-8")) + if self.server.remote_viewers.get(self.camera, False): + await self.server.request_keyframe(self.camera) + + def normalize_url(self): + if self.athena_host.startswith(("ws://", "wss://", "http://", "https://")): + return self.athena_host.replace("http://", "ws://", 1).replace("https://", "wss://", 1).rstrip("/") + return "ws://" + self.athena_host.rstrip("/") + + async def read_loop(self): + while True: + opcode, payload = await self.server.read_ws_frame(self.reader) + if opcode == 0x8: + raise ConnectionError("remote closed") + if opcode == 0x2: + cmd, body = self.server.parse_ingest_packet(payload) + if cmd == 2 and body: + had_viewer = self.server.remote_viewers.get(self.camera, False) + self.server.remote_viewers[self.camera] = body[:1] == b"1" + await self.server.notify_ingests(self.camera) + if not had_viewer and self.server.remote_viewers[self.camera]: + self.waiting_for_keyframe = True + self.clear_pending_frames() + await self.server.request_keyframe(self.camera) + elif cmd == 3: + self.waiting_for_keyframe = True + self.clear_pending_frames() + await self.server.request_keyframe(self.camera) + + async def send_h264(self, payload): + is_keyframe = self.server.is_h264_keyframe(payload) + if self.waiting_for_keyframe: + if not is_keyframe: + return + self.waiting_for_keyframe = False + + now = time.monotonic() + if self.pending_frames and now - self.pending_frames[0][0] > REMOTE_MAX_LATENCY: + self.clear_pending_frames() + if not is_keyframe: + self.waiting_for_keyframe = True + await self.server.request_keyframe(self.camera) + return + + self.pending_frames.append((now, payload)) + self.pending_event.set() + + def clear_pending_frames(self): + self.pending_frames.clear() + self.pending_event.clear() + + async def send_loop(self): + while True: + await self.pending_event.wait() + while self.pending_frames: + queued_at, frame = self.pending_frames.popleft() + if time.monotonic() - queued_at > REMOTE_MAX_LATENCY: + self.waiting_for_keyframe = True + self.clear_pending_frames() + await self.server.request_keyframe(self.camera) + break + await asyncio.wait_for( + self.send_packet(1, frame), + timeout=REMOTE_MAX_LATENCY, + ) + if not self.pending_frames: + self.pending_event.clear() + + async def send_packet(self, cmd, payload): + if not self.connected or not self.writer: + return + packet = self.server.make_ingest_packet(cmd, payload) + async with self.write_lock: + await self.write_client_ws_frame(packet, opcode=0x2) + + async def write_client_ws_frame(self, payload, opcode=0x2): + header = bytearray([0x80 | opcode]) + length = len(payload) + if length <= 125: + header.append(0x80 | length) + elif length <= 0xffff: + header.extend((0x80 | 126, *struct.pack("!H", length))) + else: + header.extend((0x80 | 127, *struct.pack("!Q", length))) + + mask = os.urandom(4) + masked = bytes(byte ^ mask[index % 4] for index, byte in enumerate(payload)) + self.writer.write(bytes(header) + mask + masked) + await self.writer.drain() + + +async def async_main(): + server = MjpegWebsocketServer(PUSH_FPS) + await server.start_remote_clients() + tcp_server = await asyncio.start_server(server.handle_client, HOST, PORT) + print(f"H.264 websocket server listening on http://{HOST}:{PORT}, fps={PUSH_FPS:g}") + async with tcp_server: + await tcp_server.serve_forever() + + +def main(): + asyncio.run(async_main()) + + +if __name__ == "__main__": + main() diff --git a/opserver b/opserver new file mode 100644 index 0000000..e91f4e2 Binary files /dev/null and b/opserver differ diff --git a/opserver.exe b/opserver.exe index a30c752..cf668b3 100644 Binary files a/opserver.exe and b/opserver.exe differ diff --git a/static/index.html b/static/index.html index e9597fe..59869b8 100644 --- a/static/index.html +++ b/static/index.html @@ -174,15 +174,183 @@ window.location = url; } - function showSentry() { - did = localStorage.getItem("dongleId"); - url = "FRP_SVR/"+String(did) - console.log(url) - window.location = url; - } - - - function recordOperationTime(){ + function showSentry() { + did = localStorage.getItem("dongleId"); + url = "http://192.168.1.113:10201/"+String(did) + console.log(url) + window.location = url; + } + + let h264Ws = null; + let h264Decoder = null; + let h264Configured = false; + let h264Timestamp = 0; + let h264Frames = 0; + + function findH264NalUnits(data) { + const units = []; + let start = -1; + let index = 0; + while (index + 3 < data.length) { + let prefixLength = 0; + if (data[index] === 0 && data[index + 1] === 0 && data[index + 2] === 1) { + prefixLength = 3; + } else if (index + 4 < data.length && + data[index] === 0 && data[index + 1] === 0 && + data[index + 2] === 0 && data[index + 3] === 1) { + prefixLength = 4; + } + if (!prefixLength) { + index += 1; + continue; + } + if (start >= 0) { + units.push(data.subarray(start, index)); + } + start = index + prefixLength; + index = start; + } + if (start >= 0 && start < data.length) { + units.push(data.subarray(start)); + } + return units; + } + + function inspectH264(data) { + let key = false; + let codec = null; + for (const nal of findH264NalUnits(data)) { + if (!nal.length) { + continue; + } + const type = nal[0] & 0x1f; + key = key || type === 5; + if (type === 7 && nal.length >= 4) { + codec = "avc1." + [nal[1], nal[2], nal[3]] + .map(function(value) { return value.toString(16).padStart(2, "0"); }) + .join("").toUpperCase(); + } + } + return {key: key, codec: codec}; + } + + function closeH264Decoder() { + if (h264Decoder) { + try { + h264Decoder.close(); + } catch (_) { + } + } + h264Decoder = null; + h264Configured = false; + h264Timestamp = 0; + } + + function createH264Decoder(camera) { + if (!("VideoDecoder" in window)) { + document.getElementById("h264Status").innerText = "WebCodecs H.264 is not supported"; + return false; + } + const canvas = document.getElementById("h264Frame"); + const context = canvas.getContext("2d"); + h264Decoder = new VideoDecoder({ + output: function(frame) { + if (canvas.width !== frame.displayWidth || canvas.height !== frame.displayHeight) { + canvas.width = frame.displayWidth; + canvas.height = frame.displayHeight; + } + context.drawImage(frame, 0, 0, canvas.width, canvas.height); + frame.close(); + h264Frames += 1; + document.getElementById("h264Status").innerText = "Live " + camera + " / " + h264Frames + " frames"; + }, + error: function(error) { + console.error("H.264 decoder error", error); + document.getElementById("h264Status").innerText = "H.264 decode error"; + closeH264Decoder(); + } + }); + return true; + } + + function playH264(camera) { + const dongleId = localStorage.getItem("dongleId"); + if (!dongleId) { + alert("Please input dongle ID first."); + return; + } + stopH264(); + document.getElementById("h264Panel").style.display = ""; + document.getElementById("h264Status").innerText = "Connecting " + camera; + h264Frames = 0; + + const protocol = location.protocol === "https:" ? "wss:" : "ws:"; + h264Ws = new WebSocket(protocol + "//" + location.host + "/h264/stream?dongle_id=" + encodeURIComponent(dongleId) + "&camera=" + encodeURIComponent(camera)); + h264Ws.binaryType = "arraybuffer"; + h264Ws.onopen = function() { + document.getElementById("h264Status").innerText = "Waiting for H.264 keyframe " + camera; + }; + h264Ws.onmessage = function(event) { + const data = new Uint8Array(event.data); + const info = inspectH264(data); + if (!h264Configured) { + if (!info.key || !info.codec || (!h264Decoder && !createH264Decoder(camera))) { + return; + } + try { + h264Decoder.configure({ + codec: info.codec, + optimizeForLatency: true, + hardwareAcceleration: "prefer-hardware" + }); + h264Configured = true; + } catch (error) { + console.error("H.264 decoder configure error", error); + document.getElementById("h264Status").innerText = "Unsupported H.264 stream"; + closeH264Decoder(); + return; + } + } + try { + h264Decoder.decode(new EncodedVideoChunk({ + type: info.key ? "key" : "delta", + timestamp: h264Timestamp, + data: data + })); + h264Timestamp += 50000; + } catch (error) { + console.error("H.264 decode submission error", error); + closeH264Decoder(); + } + }; + h264Ws.onclose = function() { + document.getElementById("h264Status").innerText = "Stopped"; + }; + h264Ws.onerror = function() { + h264Ws.close(); + }; + recordOperationTime(); + } + + function stopH264() { + if (h264Ws) { + h264Ws.onclose = null; + h264Ws.close(); + h264Ws = null; + } + closeH264Decoder(); + const canvas = document.getElementById("h264Frame"); + if (canvas) { + canvas.getContext("2d").clearRect(0, 0, canvas.width, canvas.height); + } + const status = document.getElementById("h264Status"); + if (status) { + status.innerText = "Stopped"; + } + } + + + function recordOperationTime(){ strSec = Math.floor(Date.now() / 1000) localStorage.setItem("operationTime", strSec) } @@ -237,11 +405,22 @@       - - - -



-
+ +

+ +    + +    + + + +



+ + +
diff --git a/static/replay.html b/static/replay.html index 22c1523..83b1f15 100644 --- a/static/replay.html +++ b/static/replay.html @@ -1,273 +1,630 @@ - - - - - - Route replay - - - - -
-
-
- -
-
- - -
- -
- - -
-
- - - - - - - - \ No newline at end of file + + + + + + Route replay + + + + +
+
+
GPS数据加载中...
+
+
+ +
+
+ +
+
+ +
+
+ +
+
+ + + + + + + +