diff --git a/.devcontainer/.bash_history.d/.gitkeep b/.devcontainer/.bash_history.d/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/.devcontainer/.claude/.gitkeep b/.devcontainer/.claude/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/.devcontainer/.local/.gitkeep b/.devcontainer/.local/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/.devcontainer/.vscode-server/.gitkeep b/.devcontainer/.vscode-server/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/.devcontainer/helper.sh b/.devcontainer/helper.sh new file mode 100755 index 00000000..32f0450b --- /dev/null +++ b/.devcontainer/helper.sh @@ -0,0 +1,41 @@ +#!/bin/bash +set -eEu -o pipefail + +scriptdir=$(dirname $(realpath $0)) + +command=${1:-} +workspace=${2:-} +shift 2 + +# echo "scriptdir=${scriptdir}" +# echo "command=${command}" +# echo "workspace=${workspace}" +# echo "args=$*" + +PERSISTENT_DIRS=".local .claude .bash_history.d .vscode-server" + +case ${command} in + initializeCommand) + # Make the persistent directories (see compose.yaml .services.dev.volumes) + for dir in ${PERSISTENT_DIRS}; do + if [ ! -d ${workspace}/.devcontainer/.persist/${dir} ]; then + echo "Making ${workspace}/.devcontainer/.persist/${dir}" + mkdir -p ${workspace}/.devcontainer/.persist/${dir} + fi + done + # Thank you Claudine, for using .claude AND .claude.json... :-/ + if [ ! -f ${workspace}/.devcontainer/.persist/.claude.json ]; then + echo "Making ${workspace}/.devcontainer/.persist/.claude.json" + echo "{}" > ${workspace}/.devcontainer/.persist/.claude.json + fi + ;; + onCreateCommand) + id + ;; + *) + echo "bad command: ${command}" + exit 1 + ;; +esac + +exit 0 diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index c6b22537..5496b0f3 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -12,6 +12,9 @@ // compare .services.jazzy-dev.volumes in ../../docker/docker-compose.yaml "workspaceFolder": "/home/fpsdk/fixposition-sdk", + "initializeCommand": "${localWorkspaceFolder}/.devcontainer/helper.sh initializeCommand ${localWorkspaceFolder}", + "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}", + // Deliberately empty, see fpsdk.code-workspace instead "customizations": {} } diff --git a/.devcontainer/lyrical/devcontainer.json b/.devcontainer/lyrical/devcontainer.json index c1f529fe..62fc9bee 100644 --- a/.devcontainer/lyrical/devcontainer.json +++ b/.devcontainer/lyrical/devcontainer.json @@ -12,6 +12,9 @@ // compare .services.lyrical-dev.volumes in ../../docker/docker-compose.yaml "workspaceFolder": "/home/fpsdk/fixposition-sdk", + "initializeCommand": "${localWorkspaceFolder}/.devcontainer/helper.sh initializeCommand ${localWorkspaceFolder}", + "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}", + // Deliberately empty, see fpsdk.code-workspace instead "customizations": {} } diff --git a/.devcontainer/noetic/devcontainer.json b/.devcontainer/noetic/devcontainer.json index 1722256e..f0517650 100644 --- a/.devcontainer/noetic/devcontainer.json +++ b/.devcontainer/noetic/devcontainer.json @@ -12,6 +12,9 @@ // compare .services.noetic-dev.volumes in ../../docker/docker-compose.yaml "workspaceFolder": "/home/fpsdk/fixposition-sdk", + "initializeCommand": "${localWorkspaceFolder}/.devcontainer/helper.sh initializeCommand ${localWorkspaceFolder}", + "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}", + // Deliberately empty, see fpsdk.code-workspace instead "customizations": {} } diff --git a/.devcontainer/trixie/devcontainer.json b/.devcontainer/trixie/devcontainer.json index 38c4a56a..b8a09556 100644 --- a/.devcontainer/trixie/devcontainer.json +++ b/.devcontainer/trixie/devcontainer.json @@ -12,6 +12,9 @@ // compare .services.trixie-dev.volumes in ../../docker/docker-compose.yaml "workspaceFolder": "/home/fpsdk/fixposition-sdk", + "initializeCommand": "${localWorkspaceFolder}/.devcontainer/helper.sh initializeCommand ${localWorkspaceFolder}", + "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}", + // Deliberately empty, see fpsdk.code-workspace instead "customizations": {} } diff --git a/.gitignore b/.gitignore index 3c704834..7b3977d9 100644 --- a/.gitignore +++ b/.gitignore @@ -15,13 +15,8 @@ CMakeCache.txt *.fpl.gz *.bag *.bak -/.devcontainer/.bash_history.d -/.devcontainer/.claude -/.devcontainer/.local -/.devcontainer/.vscode-server -/.devcontainer/.claude -/.devcontainer/.local -.devcontainer/.gdb/__pycache__ +/.devcontainer/.persist +__pycache__ /ws /ws2 /ros_ws @@ -46,6 +41,9 @@ baz* /*.json /.claude /*_bag +/prompt.md +/notes.md + # We must not commit such files ever. See section FPSDK_BUILD_ROS in fpsdk_doc/fpsdk_build.hpp CATKIN_IGNORE diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 8a8a2039..0b3eac86 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -46,12 +46,15 @@ services: - ../.devcontainer/.bashrc:/home/fpsdk/.bashrc - ../.devcontainer/.bash_logout:/home/fpsdk/.bash_logout # Persistent stuff - - ../.devcontainer/.bash_history.d:/home/fpsdk/.bash_history.d - - ../.devcontainer/.vscode-server:/home/fpsdk/.vscode-server - - ../.devcontainer/.claude:/home/fpsdk/.claude - - ../.devcontainer/.local:/home/fpsdk/.local + - ../.devcontainer/.persist/.bash_history.d:/home/fpsdk/.bash_history.d + - ../.devcontainer/.persist/.vscode-server:/home/fpsdk/.vscode-server + - ../.devcontainer/.persist/.claude:/home/fpsdk/.claude + - ../.devcontainer/.persist/.claude.json:/home/fpsdk/.claude.json + - ../.devcontainer/.persist/.local:/home/fpsdk/.local # Workspace - ../.:/home/fpsdk/fixposition-sdk + devices: + - /dev/dri cap_add: - SYS_PTRACE # Set kernel.yama.ptrace_scope = 0 in /etc/sysctl.d/10-ptrace.conf hostname: fpsdk-trixie-dev-${USER} @@ -95,10 +98,11 @@ services: volumes: - ../.devcontainer/.bashrc:/home/fpsdk/.bashrc - ../.devcontainer/.bash_logout:/home/fpsdk/.bash_logout - - ../.devcontainer/.bash_history.d:/home/fpsdk/.bash_history.d - - ../.devcontainer/.vscode-server:/home/fpsdk/.vscode-server - - ../.devcontainer/.claude:/home/fpsdk/.claude - - ../.devcontainer/.local:/home/fpsdk/.local + - ../.devcontainer/.persist/.bash_history.d:/home/fpsdk/.bash_history.d + - ../.devcontainer/.persist/.vscode-server:/home/fpsdk/.vscode-server + - ../.devcontainer/.persist/.claude:/home/fpsdk/.claude + - ../.devcontainer/.persist/.claude.json:/home/fpsdk/.claude.json + - ../.devcontainer/.persist/.local:/home/fpsdk/.local - ../.:/home/fpsdk/fixposition-sdk cap_add: - SYS_PTRACE @@ -142,10 +146,11 @@ services: volumes: - ../.devcontainer/.bashrc:/home/fpsdk/.bashrc - ../.devcontainer/.bash_logout:/home/fpsdk/.bash_logout - - ../.devcontainer/.bash_history.d:/home/fpsdk/.bash_history.d - - ../.devcontainer/.vscode-server:/home/fpsdk/.vscode-server - - ../.devcontainer/.claude:/home/fpsdk/.claude - - ../.devcontainer/.local:/home/fpsdk/.local + - ../.devcontainer/.persist/.bash_history.d:/home/fpsdk/.bash_history.d + - ../.devcontainer/.persist/.vscode-server:/home/fpsdk/.vscode-server + - ../.devcontainer/.persist/.claude:/home/fpsdk/.claude + - ../.devcontainer/.persist/.claude.json:/home/fpsdk/.claude.json + - ../.devcontainer/.persist/.local:/home/fpsdk/.local - ../.:/home/fpsdk/fixposition-sdk cap_add: - SYS_PTRACE @@ -186,10 +191,11 @@ services: volumes: - ../.devcontainer/.bashrc:/home/fpsdk/.bashrc - ../.devcontainer/.bash_logout:/home/fpsdk/.bash_logout - - ../.devcontainer/.bash_history.d:/home/fpsdk/.bash_history.d - - ../.devcontainer/.vscode-server:/home/fpsdk/.vscode-server - - ../.devcontainer/.claude:/home/fpsdk/.claude - - ../.devcontainer/.local:/home/fpsdk/.local + - ../.devcontainer/.persist/.bash_history.d:/home/fpsdk/.bash_history.d + - ../.devcontainer/.persist/.vscode-server:/home/fpsdk/.vscode-server + - ../.devcontainer/.persist/.claude:/home/fpsdk/.claude + - ../.devcontainer/.persist/.claude.json:/home/fpsdk/.claude.json + - ../.devcontainer/.persist/.local:/home/fpsdk/.local - ../.:/home/fpsdk/fixposition-sdk cap_add: - SYS_PTRACE @@ -233,6 +239,7 @@ services: - ../.devcontainer/.bash_history.d:/home/fpsdk/.bash_history.d - ../.devcontainer/.vscode-server:/home/fpsdk/.vscode-server - ../.devcontainer/.claude:/home/fpsdk/.claude + - ../.devcontainer/.claude.json:/home/fpsdk/.claude.json - ../.devcontainer/.local:/home/fpsdk/.local - ../.:/home/fpsdk/fixposition-sdk cap_add: diff --git a/docker/scripts/install_apt_base.sh b/docker/scripts/install_apt_base.sh index 9a845893..22963f15 100755 --- a/docker/scripts/install_apt_base.sh +++ b/docker/scripts/install_apt_base.sh @@ -83,6 +83,7 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' < 0) { @@ -168,37 +175,43 @@ bool FplToolExtract::Run() break; } - // Process message - const auto log_type = fpl_msg.PayloadType(); - ProcRes res = ProcRes::OK; - switch (log_type) { // clang-format off - case FplType::LOGSTATUS: res = ProcessLogStatus(fpl_msg, do_extract, time_into_log); break; - case FplType::LOGMETA: res = ProcessLogMeta(fpl_msg, do_extract); break; - case FplType::STREAMMSG: res = ProcessStreamMsg(fpl_msg, do_extract); break; - case FplType::ROSMSGDEF: res = ProcessRosMsgDef(fpl_msg, do_extract); break; - case FplType::ROSMSGBIN: res = ProcessRosMsgBin(fpl_msg, do_extract); break; - case FplType::FILEDUMP: res = ProcessFileDump(fpl_msg, do_extract); break; - case FplType::CAMDATA: res = ProcessCamData(fpl_msg, do_extract); break; + // Queue message for processing + QueueItem item; + item.do_extract_ = do_extract; + switch (fpl_msg.PayloadType()) { + case FplType::LOGSTATUS: { + const LogStatus logstatus(fpl_msg); + if (logstatus.valid_) { + time_into_log = logstatus.log_duration_; + } + break; + } + case FplType::CAMDATA: +#if FPSDK_USE_FFMPEG + if (do_extract && do_cam_ && do_ros_) { + ok = QueueCamData(fpl_msg, item.job_, item.job_ix_); + } +#endif + break; + case FplType::LOGMETA: + case FplType::STREAMMSG: + case FplType::ROSMSGDEF: + case FplType::ROSMSGBIN: + case FplType::FILEDUMP: + break; case FplType::BLOB: case FplType::INT_D: case FplType::INT_F: case FplType::INT_X: - case FplType::UNSPECIFIED: break; - } // clang-format on + case FplType::UNSPECIFIED: + continue; + } + item.fpl_msg_ = std::move(fpl_msg); + queue_.push_back(std::move(item)); - switch (res) { - case ProcRes::OK: - break; - case ProcRes::ERROR: - if (errors >= 100) { - WARNING("Too many errors, giving up"); - ok = false; - } - break; - case ProcRes::FATAL: - WARNING("Giving up"); - ok = false; - break; + // Process what we can + if (ok) { + ok = ProcessQueue(QueueWait::NONE); } } @@ -207,6 +220,20 @@ bool FplToolExtract::Run() ok = false; } + // Process what is left +#if FPSDK_USE_FFMPEG + for (auto& entry : gop_jobs_) { + entry.second->Close(); + } + gop_jobs_.clear(); +#endif + if (ok) { + ok = ProcessQueue(QueueWait::ALL); + } + if (!ok) { + AbortQueue(); + } + // Close output files CloseAll(ok); if (do_ros_) { @@ -230,8 +257,82 @@ bool FplToolExtract::Run() // --------------------------------------------------------------------------------------------------------------------- -FplToolExtract::ProcRes FplToolExtract::ProcessLogStatus( - const common::fpl::FplMessage& fpl_msg, const bool do_extract, uint32_t& time_into_log) +bool FplToolExtract::ProcessQueue(const QueueWait wait) +{ +#if FPSDK_USE_FFMPEG + bool may_wait = (wait != QueueWait::NONE); +#else + UNUSED(wait); +#endif + while (!queue_.empty()) { + auto& item = queue_.front(); + +#if FPSDK_USE_FFMPEG + // Camera frames that are not decoded yet hold back everything + if (item.job_ && !item.job_->IsDecoded(item.job_ix_)) { + if (!may_wait) { + break; + } + may_wait = (wait == QueueWait::ALL); + } +#endif + + const auto& fpl_msg = item.fpl_msg_; + const bool do_extract = item.do_extract_; + ProcRes res = ProcRes::OK; + switch (fpl_msg.PayloadType()) { // clang-format off + case FplType::LOGSTATUS: res = ProcessLogStatus(fpl_msg, do_extract); break; + case FplType::LOGMETA: res = ProcessLogMeta(fpl_msg, do_extract); break; + case FplType::STREAMMSG: res = ProcessStreamMsg(fpl_msg, do_extract); break; + case FplType::ROSMSGDEF: res = ProcessRosMsgDef(fpl_msg, do_extract); break; + case FplType::ROSMSGBIN: res = ProcessRosMsgBin(fpl_msg, do_extract); break; + case FplType::FILEDUMP: res = ProcessFileDump(fpl_msg, do_extract); break; + case FplType::CAMDATA: res = ProcessCamData(item); break; + case FplType::BLOB: + case FplType::INT_D: + case FplType::INT_F: + case FplType::INT_X: + case FplType::UNSPECIFIED: break; + } // clang-format on + + queue_.pop_front(); + + switch (res) { + case ProcRes::OK: + break; + case ProcRes::ERROR: + errors_++; + if (errors_ >= 100) { + WARNING("Too many errors, giving up"); + return false; + } + break; + case ProcRes::FATAL: + WARNING("Giving up"); + return false; + } + } + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + +void FplToolExtract::AbortQueue() +{ +#if FPSDK_USE_FFMPEG + for (auto& item : queue_) { + if (item.job_) { + item.job_->Abort(); + } + } + gops_pending_ = 0; +#endif + queue_.clear(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +FplToolExtract::ProcRes FplToolExtract::ProcessLogStatus(const common::fpl::FplMessage& fpl_msg, const bool do_extract) { const LogStatus logstatus(fpl_msg); if (!logstatus.valid_) { @@ -240,8 +341,6 @@ FplToolExtract::ProcRes FplToolExtract::ProcessLogStatus( } TRACE("LOGSTATUS %s", logstatus.info_.c_str()); - time_into_log = logstatus.log_duration_; - if (do_extract && do_jsonl_ && !WriteJson(jsonl_name_, logstatus)) { return ProcRes::FATAL; } @@ -451,13 +550,13 @@ FplToolExtract::ProcRes FplToolExtract::ProcessFileDump(const FplMessage& fpl_ms // --------------------------------------------------------------------------------------------------------------------- -FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const FplMessage& fpl_msg, const bool do_extract) +FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const QueueItem& item) { - if (!do_extract) { + if (!item.do_extract_) { return ProcRes::OK; } - fpsdk::common::fpl::CamData camdata(fpl_msg); + fpsdk::common::fpl::CamData camdata(item.fpl_msg_); if (!camdata.valid_) { WARNING("Invalid CAMDATA"); return ProcRes::ERROR; @@ -471,13 +570,39 @@ FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const FplMessage& fpl_msg return ProcRes::FATAL; } +#if FPSDK_USE_FFMPEG + // Frames that were decoded (and stringified to JSON) in a GopJob + if (item.job_) { + auto& job = *item.job_; + auto decdata = job.GetDecoded(item.job_ix_); + job.num_done_++; + if (job.closed_ && (job.num_done_ == job.num_frames_)) { + gops_pending_--; + } + if (do_jsonl_ && do_cam_ && !WriteJson(jsonl_name_, decdata.json_)) { + return ProcRes::FATAL; + } + return ProcessAsyncDecData(std::move(decdata)); + } +#endif + if (do_jsonl_ && do_cam_ && !WriteJson(jsonl_name_, camdata)) { return ProcRes::FATAL; } - ProcRes res = ProcRes::OK; + return ProcRes::OK; +} + +// --------------------------------------------------------------------------------------------------------------------- #if FPSDK_USE_FFMPEG +bool FplToolExtract::QueueCamData(const FplMessage& fpl_msg, std::shared_ptr& job, std::size_t& job_ix) +{ + fpsdk::common::fpl::CamData camdata(fpl_msg); + if (!camdata.valid_) { + return true; // ProcessCamData() will complain + } + VideoCodec codec = VideoCodec::UNSPECIFIED; switch (camdata.fmt_) { // clang-format off case CamDataFmt::H264_NAL: codec = VideoCodec::H264; break; @@ -489,46 +614,138 @@ FplToolExtract::ProcRes FplToolExtract::ProcessCamData(const FplMessage& fpl_msg case CamDataFmt::NV12: case CamDataFmt::RGB24: break; } // clang-format on - while (do_cam_ && do_ros_ && (codec != VideoCodec::UNSPECIFIED)) { - auto entry = decoder_states_.find(name); - if (entry == decoder_states_.end()) { - // We can only start decoding from the first I-frame onwards - if (camdata.frm_ != CamDataFrm::I_FRAME) { - break; + if (codec == VideoCodec::UNSPECIFIED) { + return true; + } + + const std::string name = Sprintf( + "%s-%s-%s", CamIdToStr(camdata.cam_id_), CamDataTypeToStr(camdata.type_), CamDataFmtToStr(camdata.fmt_)); + auto entry = gop_jobs_.find(name); + + // An I-frame starts a new GOP, which is decoded independently of the previous one + if (camdata.frm_ == CamDataFrm::I_FRAME) { + if (entry != gop_jobs_.end()) { + auto& prev = *entry->second; + prev.Close(); + if (prev.num_done_ < prev.num_frames_) { + gops_pending_++; } - AsyncDecState state; - state.dec_ = CreateVideoFrameDecoder({ name, codec, opts_.pixelfmt_, opts_.scale_ }); - entry = decoder_states_.emplace(name, std::move(state)).first; + gop_jobs_.erase(entry); } - if (!entry->second.dec_) { - WARNING("No decoder for %s", name.c_str()); - return ProcRes::FATAL; + // Don't run too many decoders at the same time, and don't queue too much decoded data + while (gops_pending_ >= max_gops_pending_) { + if (!ProcessQueue(QueueWait::HEAD)) { + return false; + } } - auto& dec = entry->second.dec_; - auto& fut = entry->second.fut_; - // Wait for decoding results from previous frame - if (fut.valid()) { - res = ProcessAsyncDecData(fut.get()); - } + // Each decoder makes its own hw device, if hw decoding is used + const VideoDecoderParams params = { name, codec, opts_.pixelfmt_, opts_.scale_, opts_.accel_ }; + entry = gop_jobs_.emplace(name, std::make_shared(params, do_jsonl_)).first; + } + // We can only start decoding from the first I-frame onwards + else if (entry == gop_jobs_.end()) { + return true; + } + + if (entry->second->num_frames_ >= MAX_GOP_SIZE) { + WARNING("Too many frames (> %" PRIuMAX ") without an I-frame in %s", MAX_GOP_SIZE, name.c_str()); + return false; + } + job = entry->second; + job_ix = job->AddFrame(std::move(camdata)); + + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + +FplToolExtract::GopJob::GopJob(const VideoDecoderParams& params, const bool do_json) /* clang-format off */ : + do_json_ { do_json } // clang-format on +{ + fut_ = std::async(std::launch::async, [this, params]() { Run(params); }); +} - // Decode this frame - fut = std::async(std::launch::async, [&dec, /*copy!*/ camdata]() -> AsyncDecData { - return { camdata, dec->DecodeFrame(camdata.data_), dec->IsOkay() }; - }); +FplToolExtract::GopJob::~GopJob() +{ + Abort(); +} - break; +std::size_t FplToolExtract::GopJob::AddFrame(fpsdk::common::fpl::CamData&& camdata) +{ + { + std::unique_lock lock(mutex_); + todo_.push_back(std::move(camdata)); } -#endif + cond_.notify_all(); + return num_frames_++; +} - return res; +void FplToolExtract::GopJob::Close() +{ + { + std::unique_lock lock(mutex_); + finish_ = true; + } + cond_.notify_all(); + closed_ = true; +} + +void FplToolExtract::GopJob::Abort() +{ + { + std::unique_lock lock(mutex_); + finish_ = true; + abort_ = true; + } + cond_.notify_all(); +} + +bool FplToolExtract::GopJob::IsDecoded(const std::size_t ix) +{ + std::unique_lock lock(mutex_); + return ix < done_.size(); +} + +FplToolExtract::AsyncDecData FplToolExtract::GopJob::GetDecoded(const std::size_t ix) +{ + std::unique_lock lock(mutex_); + cond_.wait(lock, [this, ix]() { return ix < done_.size(); }); + return std::move(done_[ix]); +} + +void FplToolExtract::GopJob::Run(const VideoDecoderParams& params) +{ + auto dec = CreateVideoFrameDecoder(params); + while (true) { + std::unique_lock lock(mutex_); + cond_.wait(lock, [this]() { return !todo_.empty() || finish_; }); + if (todo_.empty() || abort_) { + break; + } + AsyncDecData decdata = { std::move(todo_.front()), std::nullopt, false, "" }; + todo_.pop_front(); + lock.unlock(); + + if (dec) { + decdata.img_ = dec->DecodeFrame(decdata.camdata_.data_); + decdata.dec_is_okay_ = dec->IsOkay(); + } + if (do_json_) { + decdata.json_ = nlohmann::json(decdata.camdata_).dump(); + } + + lock.lock(); + done_.push_back(std::move(decdata)); + lock.unlock(); + cond_.notify_all(); + } } // --------------------------------------------------------------------------------------------------------------------- -#if FPSDK_USE_FFMPEG -FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(const FplToolExtract::AsyncDecData& decdata) +FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(FplToolExtract::AsyncDecData&& decdata) { auto& camdata = decdata.camdata_; if (!decdata.img_) { @@ -560,7 +777,7 @@ FplToolExtract::ProcRes FplToolExtract::ProcessAsyncDecData(const FplToolExtract case PixelFmt::GBRP: rosmsg.encoding = "8UC3"; rosmsg.step = img.width_; break; case PixelFmt::UNSPECIFIED: break; } // clang-format on - rosmsg.data = { img.data_.data(), img.data_.data() + img.data_.size() }; + rosmsg.data = std::move(img.data_); // Abuse pixel (0, 0) to store the exposure duration in [0.1ms] if (!rosmsg.data.empty()) { const uint32_t dt01ms = camdata.dt_ / 100000; // [ns] -> [0.1ms] @@ -596,6 +813,17 @@ bool FplToolExtract::WriteJson(const std::string& name, const nlohmann::json& js // --------------------------------------------------------------------------------------------------------------------- +bool FplToolExtract::WriteJson(const std::string& name, const std::string& json) +{ + auto f = GetOutputFile(name); + if (!f) { + return false; + } + return f->Write(json) && f->Write("\n"); +} + +// --------------------------------------------------------------------------------------------------------------------- + bool FplToolExtract::WriteStreamMsg(const std::string& name, const StreamMsg& streammsg, const ParserMsg& parsermsg) { parsermsg.MakeInfo(); diff --git a/fpsdk_apps/fpltool/fpltool_extract.hpp b/fpsdk_apps/fpltool/fpltool_extract.hpp index e85906f7..514b4e82 100644 --- a/fpsdk_apps/fpltool/fpltool_extract.hpp +++ b/fpsdk_apps/fpltool/fpltool_extract.hpp @@ -14,8 +14,13 @@ #define __FPSDK_APPS_FPLTOOL_FPLTOOL_EXTRACT_HPP__ /* LIBC/STL */ +#include +#include #include #include +#include +#include +#include #include #include #include @@ -29,6 +34,7 @@ #include #include #include +#include #include /* PACKAGE */ @@ -60,13 +66,12 @@ class FplToolExtract // FPL // clang-format off enum class ProcRes { OK, ERROR, FATAL }; // clang-format off - ProcRes ProcessLogStatus(const common::fpl::FplMessage& fpl_msg, const bool do_extract, uint32_t& time_into_log); + ProcRes ProcessLogStatus(const common::fpl::FplMessage& fpl_msg, const bool do_extract); ProcRes ProcessLogMeta(const common::fpl::FplMessage& fpl_msg, const bool do_extract); ProcRes ProcessRosMsgDef(const common::fpl::FplMessage& fpl_msg, const bool do_extract); ProcRes ProcessRosMsgBin(const common::fpl::FplMessage& fpl_msg, const bool do_extract); ProcRes ProcessStreamMsg(const common::fpl::FplMessage& fpl_msg, const bool do_extract); ProcRes ProcessFileDump(const common::fpl::FplMessage& fpl_msg, const bool do_extract); - ProcRes ProcessCamData(const common::fpl::FplMessage& fpl_msg, const bool do_extract); // LOGMETA bool have_logmeta_ = false; @@ -90,16 +95,62 @@ class FplToolExtract common::fpl::CamData camdata_; std::optional img_; bool dec_is_okay_ = true; + std::string json_; }; - struct AsyncDecState + + // Decoding of one group of pictures (GOP, one I-frame and the P-frames that follow it), using its own decoder + // in its own thread. The main thread adds the frames as it reads them from the log and collects the decoded + // frames in the same order. + class GopJob : private common::types::NoCopyNoMove { - common::video::VideoFrameDecoderPtr dec_; - std::future fut_; + public: + GopJob(const common::video::VideoDecoderParams& params, const bool do_json); + ~GopJob(); + std::size_t AddFrame(common::fpl::CamData&& camdata); // Add frame to decode, returns the frame index + void Close(); // No more frames will be added (end of GOP) + void Abort(); // Stop decoding as soon as possible + bool IsDecoded(const std::size_t ix); // Check if frame is decoded + AsyncDecData GetDecoded(const std::size_t ix); // Get decoded frame, waits for it if necessary + std::size_t num_frames_ = 0; // Number of frames added (main thread only) + std::size_t num_done_ = 0; // Number of frames collected (main thread only) + bool closed_ = false; // No more frames will be added (main thread only) + + private: + void Run(const common::video::VideoDecoderParams& params); + std::mutex mutex_; + std::condition_variable cond_; + std::deque todo_; + std::vector done_; + bool finish_ = false; + bool abort_ = false; + const bool do_json_; + std::future fut_; // Must be last, so that the thread is gone before the other members are }; - std::map decoder_states_; + std::map> gop_jobs_; // The GOP we're currently reading, per stream + std::size_t gops_pending_ = 0; // Number of complete GOPs not fully written yet + std::size_t max_gops_pending_ = 1; + + bool QueueCamData(const common::fpl::FplMessage& fpl_msg, std::shared_ptr& job, std::size_t& job_ix); + ProcRes ProcessAsyncDecData(AsyncDecData&& decdata); +#endif - ProcRes ProcessAsyncDecData(const AsyncDecData& decdata); + // Messages are queued and processed in the order they are in the log. Camera frames that need decoding are held + // back, along with everything that follows them, until they are decoded. + struct QueueItem + { + common::fpl::FplMessage fpl_msg_; + bool do_extract_ = true; +#if FPSDK_USE_FFMPEG + std::shared_ptr job_; + std::size_t job_ix_ = 0; #endif + }; + std::deque queue_; + enum class QueueWait { NONE, HEAD, ALL }; + bool ProcessQueue(const QueueWait wait); + ProcRes ProcessCamData(const QueueItem& item); + void AbortQueue(); + std::size_t errors_ = 0; // Output files #if FPSDK_USE_ROS2 @@ -111,6 +162,7 @@ class FplToolExtract common::path::OutputFile* GetOutputFile(const std::string& name); bool WriteData(const std::string& name, const std::vector& data); bool WriteJson(const std::string& name, const nlohmann::json& json); + bool WriteJson(const std::string& name, const std::string& json); bool WriteStreamMsg( const std::string& name, const common::fpl::StreamMsg& streammsg, const common::parser::ParserMsg& parsermsg); void CloseAll(const bool ok = false); diff --git a/fpsdk_apps/fpltool/fpltool_opts.hpp b/fpsdk_apps/fpltool/fpltool_opts.hpp index 8f21c5c1..9a05b4fd 100644 --- a/fpsdk_apps/fpltool/fpltool_opts.hpp +++ b/fpsdk_apps/fpltool/fpltool_opts.hpp @@ -16,6 +16,7 @@ /* LIBC/STL */ #include #include +#include #include /* EXTERNAL */ @@ -54,6 +55,8 @@ class FplToolOptions : public common::app::ProgramOptions { 'e', true, "formats" }, { 's', true, "scale" }, { 't', true, "pixelfmt" }, + { 'j', true, "jobs" }, + { 'a', true, "accel" }, }) {}; // clang-format on /** @@ -88,6 +91,8 @@ class FplToolOptions : public common::app::ProgramOptions #if FPSDK_USE_FFMPEG double scale_ = 1.0; //!< Scale factor for decoded video frames common::video::PixelFmt pixelfmt_ = common::video::PixelFmt::RGB24; //!< Pixel format for decoded video frames + uint32_t jobs_ = 0; //!< Number of parallel video decoders (0 = auto) + common::video::HwAccel accel_ = common::video::HwAccel::SW; //!< Hw acceleration for video decoders #endif // clang-format on @@ -125,6 +130,8 @@ class FplToolOptions : public common::app::ProgramOptions " -e, --formats -- Comma-separated list of output formats for the extract (default: jsonl,raw,file)\n" " -s, --scale -- Scale factor for decoded video frames: 0.1 - 1.0 (default: 1.0)\n" " -t, --pixelfmt -- Pixel format for decoded video frames: Y8, RGB24, GBRP (default: RGB24)\n" + " -j, --jobs -- Number of video decoders to run in parallel (default: number of CPUs - 2, at least 1)\n" + " -a, --accel -- Hardware acceleration for video decoders: SW (default, recommended), HW or AUTO\n" " -- The command, see below\n" " -- The .fpl (or .fpl.gz) file to process\n" " \n" @@ -160,7 +167,7 @@ class FplToolOptions : public common::app::ProgramOptions "\n" " extract -- Extract the data in a .fpl file\n" "\n" - " fpltool [-vqpPfocSDst] [-e ] extract \n" + " fpltool [-vqpPfocSDstja] [-e ] extract \n" "\n" " The data is extracted to different files in the current directory. The files are named like the\n" " with added suffixes and different file extension, depending on the kind of data that\n" @@ -373,6 +380,27 @@ class FplToolOptions : public common::app::ProgramOptions #else WARNING("Cannot use --pixelfmt, this fpltool is not compiled with FFmpeg"); ok = false; +#endif + break; + case 'j': +#if FPSDK_USE_FFMPEG + if (!common::string::StrToValue(argument, jobs_) || (jobs_ < 1)) { + ok = false; + } +#else + WARNING("Cannot use --jobs, this fpltool is not compiled with FFmpeg"); + ok = false; +#endif + break; + case 'a': +#if FPSDK_USE_FFMPEG + accel_ = HwAccelFromStrOr(argument.c_str(), common::video::HwAccel::UNSPECIFIED); + if (accel_ == common::video::HwAccel::UNSPECIFIED) { + ok = false; + } +#else + WARNING("Cannot use --accel, this fpltool is not compiled with FFmpeg"); + ok = false; #endif break; default: @@ -416,6 +444,14 @@ class FplToolOptions : public common::app::ProgramOptions progress_ = 1; } +#if FPSDK_USE_FFMPEG + // Default number of jobs: leave two CPUs for the rest (reading, writing) + if (jobs_ == 0) { + const uint32_t num_cpus = std::thread::hardware_concurrency(); + jobs_ = (num_cpus > 2 ? num_cpus - 2 : 1); + } +#endif + // Debug DEBUG("command_ = '%s'", command_str_.c_str()); for (std::size_t ix = 0; ix < inputs_.size(); ix++) { @@ -435,6 +471,8 @@ class FplToolOptions : public common::app::ProgramOptions #if FPSDK_USE_FFMPEG DEBUG("scale = %.1f", scale_); DEBUG("pixelfmt = %s", common::video::PixelFmtToStr(pixelfmt_)); + DEBUG("jobs = %d", jobs_); + DEBUG("accel = %s", common::video::HwAccelToStr(accel_)); #endif return ok; } diff --git a/fpsdk_common/include/fpsdk_common/video.hpp b/fpsdk_common/include/fpsdk_common/video.hpp index 2b0021d7..234d9bff 100644 --- a/fpsdk_common/include/fpsdk_common/video.hpp +++ b/fpsdk_common/include/fpsdk_common/video.hpp @@ -9,13 +9,40 @@ * * @file * @brief Fixposition SDK: Video frame decoding - * + */ +// clang-format off +/** * @page FPSDK_COMMON_VIDEO Video frame decoding * * **API**: fpsdk_common/video.hpp and fpsdk::common::video * * @note This is only available if compiled with FFmpeg, see @ref FPSDK_BUILD_DEPS. + * + * @section FPSDK_COMMON_VIDEO_HWACCEL Hardware vs. software decoding + * + * Hardware decoding (fpsdk::common::video::HwAccel::HW, currently VA-API) is not necessarily faster than software + * decoding (fpsdk::common::video::HwAccel::SW). The following are wall-clock times for "fpltool extract" of a 32 s + * recording with two cameras (H.265, 1024x768, 25 fps, 10 frames per GOP, 1547 frames in total) decoded to RGB24 + * (a 3.5 GB ROS1 bag), using different numbers of parallel decoders (-j). Measured on an AMD Ryzen 7 PRO 8840HS + * (8 cores, 16 threads) with Radeon 780M graphics, FFmpeg 7.1, Release build, median of three runs: + * + * | Decoders | -e jsonl,raw,file,ros,cam -a SW | -e jsonl,raw,file,ros,cam -a HW | -e ros,cam -a SW | -e ros,cam -a HW | + * |----------|---------------------------------|---------------------------------|------------------|------------------| + * | -j 1 | 5.3 s | 5.9 s | 4.4 s | 5.7 s | + * | -j 2 | 4.7 s | 5.8 s | 4.7 s | 6.2 s | + * | -j 4 | 4.2 s | 6.1 s | 4.2 s | 5.6 s | + * | -j 8 | 4.2 s | 6.5 s | 4.0 s | 5.5 s | + * + * Findings: + * + * - Software decoding is faster than hardware decoding in all cases + * - Decoding this kind of video is cheap on the CPU (about 1.2 ms per P-frame and 8 ms per I-frame on one core), + * and it scales with the number of decoders. + * - The hardware decoder is limited by getting the decoded frames back from the GPU, and the GPU does not scale with + * the number of decoders. The conversion to the output pixel format is done in software in both cases. + * - The results may be different for other video (larger frames, H.264, longer GOPs), GPUs and drivers. */ +// clang-format on #ifndef __FPSDK_COMMON_VIDEO_HPP__ #define __FPSDK_COMMON_VIDEO_HPP__ diff --git a/fpsdk_common/src/fpl.cpp b/fpsdk_common/src/fpl.cpp index 155fa7e8..3deb55be 100644 --- a/fpsdk_common/src/fpl.cpp +++ b/fpsdk_common/src/fpl.cpp @@ -755,7 +755,7 @@ CamData::CamData(const FplMessage& log_msg) info_ = string::Sprintf("rec_time=%.3f cam_id=%s type=%s fmt=%s frm=%s %" PRIu32 "x%" PRIu32 " seq=%" PRIuMAX " ts=%.3f size=%" PRIuMAX, - rec_time_.ToSec() * 1e-9, CamIdToStr(cam_id_), CamDataTypeToStr(type_), CamDataFmtToStr(fmt_), + rec_time_.ToSec(), CamIdToStr(cam_id_), CamDataTypeToStr(type_), CamDataFmtToStr(fmt_), CamDataFrmToStr(frm_), width_, height_, seq_, (double)ts_ * 1e-9, data_.size()); } else { WARNING("CamData: invalid message"); diff --git a/fpsdk_common/src/video.cpp b/fpsdk_common/src/video.cpp index abddd679..770cdbf2 100644 --- a/fpsdk_common/src/video.cpp +++ b/fpsdk_common/src/video.cpp @@ -14,6 +14,7 @@ /* LIBC/STL */ #include #include +#include #include #include @@ -210,14 +211,11 @@ class VideoFrameDecoderImpl : public VideoFrameDecoder, private NoCopyNoMove std::optional DecodeFrame(const uint8_t* data, const std::size_t size) override final; bool IsOkay() const override final; - enum AVPixelFormat GetHwFormat(const enum AVPixelFormat* fmts) const; - private: // clang-format off enum class State { DEINIT, INIT_AV, INIT_GRAPH, ERROR }; State state_ = State::DEINIT; AVHWDeviceType hw_type_ = AV_HWDEVICE_TYPE_NONE; // AV_HWDEVICE_TYPE_VAAPI - enum AVPixelFormat src_fmt_ = AV_PIX_FMT_YUVJ420P; // AV_PIX_FMT_VAAPI const AVCodec* codec_ = nullptr; AVCodecContext* ctx_ = nullptr; AVBufferRef* hw_ctx_ = nullptr; // hwdevice_ctx @@ -265,21 +263,6 @@ bool VideoFrameDecoderImpl::IsOkay() const // --------------------------------------------------------------------------------------------------------------------- -static enum AVPixelFormat GetHwFormatCb(AVCodecContext* ctx, const enum AVPixelFormat* fmts) -{ - return static_cast(ctx->opaque)->GetHwFormat(fmts); -} - -enum AVPixelFormat VideoFrameDecoderImpl::GetHwFormat(const enum AVPixelFormat* fmts) const -{ - for (const enum AVPixelFormat* p = fmts; *p != AV_PIX_FMT_NONE; ++p) { - if (*p == src_fmt_) { - return *p; - } - } - return AV_PIX_FMT_NONE; -} - bool VideoFrameDecoderImpl::InitAv() { DEBUG("VideoFrameDecoderImpl(%s) init", params_.name_.c_str()); @@ -354,14 +337,10 @@ bool VideoFrameDecoderImpl::InitAv() } hw = nullptr; } else { - // hw_ctx_ = reinterpret_cast(hw); - hw_ctx_ = av_buffer_ref(hw); + // FFmpeg's default get_format() selects the hw pixel format if the decoder can use this device. + // Otherwise it decodes in sw and we get sw frames, see InitGraph(). + hw_ctx_ = hw; hw_type_ = AV_HWDEVICE_TYPE_VAAPI; - src_fmt_ = AV_PIX_FMT_VAAPI; - // @todo this crashes.. - // ctx_->opaque = this; - // ctx_->get_format = GetHwFormatCb; - UNUSED(GetHwFormatCb); } } @@ -449,13 +428,33 @@ bool VideoFrameDecoderImpl::InitGraph() int src_width = 0; int src_height = 0; - AVPixelFormat src_format = AV_PIX_FMT_NONE; // TODO check <-> src_fmt_ + AVPixelFormat src_format = AV_PIX_FMT_NONE; AVRational src_time_base = { 1, 25 }; AVRational src_sar = { 1, 1 }; src_width = frame_->width; src_height = frame_->height; - src_format = static_cast(frame_->format); // yuvj420p, check that + src_format = static_cast(frame_->format); // sw: yuvj420p, hw: vaapi + + // We may have a hw device, but the decoder may not be able to use it (codec, profile, FFmpeg build, ...) + const bool hw_frame = (src_format == AV_PIX_FMT_VAAPI); + if (!hw_frame && (params_.accel_ == HwAccel::HW)) { + WARNING("VideoFrameDecoder(%s) no hw decoding for this video", params_.name_.c_str()); + return false; + } + if (!hw_frame && hw_ctx_) { + DEBUG("VideoFrameDecoder(%s) no hw decoding for this video, using sw decoding", params_.name_.c_str()); + } + + // Tell the user once, not for each decoder + static std::atomic_flag info_done = ATOMIC_FLAG_INIT; + if (!info_done.test_and_set()) { + if (hw_frame) { + INFO("Using hardware (%s) video decoder", av_hwdevice_get_type_name(hw_type_)); + } else { + INFO("Using software video decoder"); + } + } if ((ctx_->time_base.num > 0) && (ctx_->time_base.den > 0)) { src_time_base = ctx_->time_base; } else { @@ -506,12 +505,12 @@ bool VideoFrameDecoderImpl::InitGraph() ok = false; break; } - par->format = src_fmt_; + par->format = src_format; par->width = src_width; par->height = src_height; par->time_base = src_time_base; par->sample_aspect_ratio = src_sar; - if (hw_ctx_) { + if (hw_frame) { if (frame_->hw_frames_ctx) { // Should be present now that we have decoded the first frame par->hw_frames_ctx = av_buffer_ref(frame_->hw_frames_ctx); } else { @@ -581,9 +580,12 @@ bool VideoFrameDecoderImpl::InitGraph() # endif char filter_desc[256]; - if (hw_ctx_) { - std::snprintf(filter_desc, sizeof(filter_desc), "scale_vaapi=w=%d:h=%d:format=nv12,hwdownload,format=nv12", - dst_width, dst_height); + if (hw_frame) { + // Scale on the GPU, download the frame, convert the pixel format in sw. Download yuv420p, not nv12, as + // swscale is much slower for nv12 to rgb24. + std::snprintf(filter_desc, sizeof(filter_desc), + "scale_vaapi=w=%d:h=%d:format=yuv420p,hwdownload,format=yuv420p,scale,format=pix_fmts=%s", dst_width, + dst_height, PixelFmtToFlag(params_.fmt_)); } else { std::snprintf(filter_desc, sizeof(filter_desc), "scale=w=%d:h=%d:flags=%s,format=pix_fmts=%s", dst_width, dst_height, ScalingQualToFlag(params_.qual_), PixelFmtToFlag(params_.fmt_));