Camera streaming from PBx-A1 sensor

/**
 * \verbatim
 * ___    ___
 * \  \  /  /
 *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
 *  /  /\  \   License: see the LICENSE file
 * /__/  \__\
 * \endverbatim
 *
 * @file
 * @brief Fixposition SDK example: Fixposition SDK example: camera streaming from PBx-A1 sensor
 *
 * To build and run:
 *
 *     make
 *     ./build/fusion_epoch
 *
 * This program builds on the "parser_intro" example and details how to collect the FP_A fusion messages into
 * fusion epochs for further processing.
 *
 * This file is both the source code of this example as well as the documentation on how it works.
 */
 
/* LIBC/STL */
#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <string>
 
/* EXTERNAL */
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/extended_p_square.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/variance.hpp>
 
/* Fixposition SDK */
#include <fpsdk_common/app.hpp>
#include <fpsdk_common/cam.hpp>
#include <fpsdk_common/logging.hpp>
#include <fpsdk_common/string.hpp>
#include <fpsdk_common/time.hpp>
#include <fpsdk_common/types.hpp>
#include <fpsdk_common/video.hpp>
 
/* PACKAGE */
 
/* ****************************************************************************************************************** */
 
using namespace fpsdk::common::app;
using namespace fpsdk::common::cam;
using namespace fpsdk::common::logging;
using namespace fpsdk::common::string;
using namespace fpsdk::common::time;
using namespace fpsdk::common::video;
 
// ---------------------------------------------------------------------------------------------------------------------
 
// Statistics
struct Stats
{
    // clang-format off
    static constexpr std::array<double, 4> PROB = {{ 0.5, 0.68, 0.95, 0.997 }};
    using Accumulator = boost::accumulators::accumulator_set<double, boost::accumulators::stats<
        boost::accumulators::tag::count,
        boost::accumulators::tag::mean,
        boost::accumulators::tag::min,
        boost::accumulators::tag::max,
        boost::accumulators::tag::sum,
        boost::accumulators::tag::variance,
        boost::accumulators::tag::extended_p_square>>;
    Accumulator lat  { boost::accumulators::extended_p_square_probabilities = PROB };  // Latency receiving the data
    Accumulator size { boost::accumulators::extended_p_square_probabilities = PROB };  // Size of the data
    Accumulator exp  { boost::accumulators::extended_p_square_probabilities = PROB };  // Exposure duration
#if FPSDK_USE_FFMPEG
    Accumulator dec  { boost::accumulators::extended_p_square_probabilities = PROB };  // Time decoding video
#endif
    // clang-format on
};
 
// ---------------------------------------------------------------------------------------------------------------------
 
// Program options
class Opts : public ProgramOptions
{
    // clang-format off
   public:
    Opts() /* clang-format off */ :
        ProgramOptions("camera_streaming", { { 's', true, "sensor" }, { 'n', true, "camera" },
        { 'd', true, "data" }, { 'r', true, "rate" }, { 'c', false, "stdout" } })  // clang-format on
    {};
 
    // clang-format off
    std::string sensor_;
    CamId       camera_ = CamId::UNSPECIFIED;
    CamDataType data_   = CamDataType::UNSPECIFIED;
    int         rate_   = 1;
    bool        stdout_ = false;
    // clang-format on
 
    void PrintHelp() override final
    {
        // clang-format off
        std::fputs(
            "\n"
            "PBx-A1 camera streaming example\n"
            "\n"
            "Usage:\n"
            "\n"
            "    camera_streaming -s <sensor> -c <camera> -d <data> [-r <rate>] [-c]\n"
            "\n", stdout);
        std::fputs(COMMON_FLAGS_HELP, stdout);
        std::fputs(
            "\n"
            "    -s, --sensor   -- Hostname or IP address of sensor\n"
            "    -n, --camera   -- Which camera to use (CAM1, CAM2, ...)\n"
            "    -d, --data     -- Which data to stream (HIRES_VID, LORES_IMG, etc.)\n"
            "    -r, --rate     -- Throttling rate (default 1, >1 only for _IMG streams)\n"
            "    -c, --stdout   -- Pipe raw image/video data to stdout (careful!)\n"
            "\n"
            "Example:\n"
            "\n"
            "     camera_streaming -s 10.0.2.1 -n CAM1 -d HIRES_VID\n"
            "\n"
            "     camera_streaming -s 10.0.2.1 -n CAM1 -d LORES_IMG\n"
            "\n"
            "     timeout -s SIGINT 60 camera_streaming ...\n"
            "\n"
            "     camera_streaming -s 10.0.2.1 -n CAM1 -d HIRES_VID -c | ffmpeg -i - -c copy out.mp4\n"
            "\n"
            "     camera_streaming -s 10.0.2.1 -n CAM1 -d HIRES_VID -q -c | \\\n"
            "         mpv --cache-secs=0 --profile=low-latency --demuxer-readahead-secs=0 --untimed --cache-pause=no -\n"
            "     camera_streaming -s 10.0.2.1 -n CAM1 -d HIRES_VID -q -c | \\\n"
            "         ffplay -fflags nobuffer -flags low_delay -framedrop -probesize 32 -analyzeduration 0 -\n"
            "\n"
            "\n", stdout);
        // clang-format on
    }
 
    bool HandleOption(const Option& option, const std::string& argument) final
    {
        bool ok = true;
        switch (option.flag) {  // clang-format off
            case 's': sensor_ = argument; break;
            case 'n': camera_ = CamIdFromStrOr(argument.c_str(), CamId::UNSPECIFIED); break;
            case 'd': data_ = CamDataTypeFromStrOr(argument.c_str(), CamDataType::UNSPECIFIED); break;
            case 'r': StrToValue(argument, rate_); break;
            case 'c': stdout_ = true; break;
            default:  ok = false; break;
        }  // clang-format on
        return ok;
    }
 
    bool CheckOptions(const std::vector<std::string>& args) final
    {
        bool ok = true;
        INFO("sensor   = %s", sensor_.c_str());
        INFO("camera   = %s", CamIdToStr(camera_));
        INFO("data     = %s", CamDataTypeToStr(data_));
        INFO("rate     = %d", rate_);
        INFO("stdout   = %s", ToStr(stdout_));
        if (!args.empty() || sensor_.empty() || (camera_ == CamId::UNSPECIFIED) ||
            (data_ == CamDataType::UNSPECIFIED) || (rate_ < 1)) {
            ok = false;
        }
        return ok;
    }
};
 
// ---------------------------------------------------------------------------------------------------------------------
 
int main(int argc, char** argv)
{
#ifndef NDEBUG
    fpsdk::common::app::StacktraceHelper stacktrace;
#endif
 
#if !FPSDK_USE_FFMPEG
    WARNING("!!!!! This app is not compiled with FFmpeg support !!!!!");
#endif
 
    // We need four things (see CamStreamParams):
    //
    // 1. The sensor (hostname, IP address)
    // 2. Which camera (CAM1, CAM2, ...)
    // 3. Which data (HIRES_VID, LORES_IMG, ...)
    // 4. Rate value (must be 1 for ..._VID, can be > 1 for ..._IMG)
    Opts opts;
    if (!opts.LoadFromArgv(argc, argv)) {
        return false;
    }
    opts.LogVersion();
 
    NOTICE("Streaming...");
 
    // Create stream handle (this formally verifies the arguments)
    auto stream = CreateCamStream({ "cam", opts.sensor_, opts.camera_, opts.data_, opts.rate_ });
    if (!stream) {
        return EXIT_FAILURE;
    }
 
    // Connect to sensor
    if (!stream->Connect()) {
        return EXIT_FAILURE;
    }
 
#if FPSDK_USE_FFMPEG
    // We'll decode encoded video frames
    VideoFrameDecoderPtr decoder;
#endif
 
    // Statistics
    Stats stats;
 
    // Stream data until we get SIGINT (CTRL-c)
    SigIntHelper sigint;
    CamData data;
    std::size_t n_frames = 0;
    char info[1000];
    bool ok = true;
    const auto t_start = Time::FromClockRealtime();
    while (ok && !sigint.ShouldAbort() && stream->NextFrame(data)) {
        // We'll calculate the latency from the time we received the data (now) and the data reference time. Note that
        // this requires the computer that runs this program being timesynced with the sensor (and the sensor being
        // timesynced, too, ideally with GNSS). Any error in timesync of the sensor or the computer affects the measured
        // latency (could be to the better or to the worse). Also, we'll use the end of exposure as the reference time
        // instead of the middle of exposure.
        const auto t_recv = Time::FromClockRealtime();
        const auto t_expo = Duration::FromNSec(data.dt_);
        const auto t_data = Time::FromPosixNs(data.ts_) + (t_expo * 0.5);
        n_frames++;
 
        // Generic info
        std::size_t len = std::snprintf(info, sizeof(info),  // clang-format off
            "Frame %6" PRIuMAX ": %06" PRIuMAX " %-7s %-5s %-10s %-9s %" PRIu32 "x%" PRIu32 " %-7s %6" PRIuMAX " %s %4.1f",
            n_frames, data.seq_, data.valid_ ? "valid" : "invalid", CamIdToStr(data.cam_id_),
            CamDataTypeToStr(data.type_), CamDataFmtToStr(data.fmt_), data.width_, data.height_,
            CamDataFrmToStr(data.frm_), data.data_.size(), t_data.StrIsoTime(3).c_str(),
            Duration::FromNSec(data.dt_).GetSec() * 1e3);  // clang-format on
 
        // Update statistics
        stats.size((double)data.data_.size() / 1024.0);  // [KiB]
        stats.exp((double)t_expo.GetSec() * 1e3);        // [ms]
 
        // Latency
        if (data.valid_) {
            const double lat = (t_recv - t_data).GetSec() * 1e3;  // [ms]
            len += std::snprintf(&info[len], sizeof(info) - len, " -- latency: %+5.1f", lat);
            stats.lat(lat);
        }
 
        // -------------------------------------------------------
        // Now we have all the raw data and meta data in "data"...
        // -------------------------------------------------------
 
        if (opts.stdout_) {
            write(fileno(stdout), data.data_.data(), data.data_.size());
        }
 
#if FPSDK_USE_FFMPEG
        // Decode video
        if (data.fmt_ == CamDataFmt::H265_NAL) {
            // We can only start decoding from the first I-frame onwards
            if (!decoder && (data.frm_ == CamDataFrm::I_FRAME)) {
                decoder = CreateVideoFrameDecoder({ "dec", VideoCodec::H265, PixelFmt::RGB24 });
            }
            if (decoder) {
                TicToc tt;
                const auto img = decoder->DecodeFrame(data.data_);
 
                // Decoding adds latency
                if (img) {
                    const auto lat = tt.Toc().GetSec() * 1e3;  // [ms]
                    len += std::snprintf(&info[len], sizeof(info) - len, " -- decode: %+5.1f", lat);
                    stats.dec(lat);
                } else {
                    len += std::snprintf(&info[len], sizeof(info) - len, " -- DECODE FAIL");
                    continue;
                }
 
                // ----------------------------------------------------------------------------------------
                // Now we have the decoded image in "img"... (and all the raw data and meta data in "data")
                // ----------------------------------------------------------------------------------------
            }
        }
#endif
 
        INFO("%s", info);
    }
    const auto t_end = Time::FromClockRealtime();
    const auto t_str = (t_end - t_start).GetSec();
 
    stream->Disconnect();
 
    // Print stats
    NOTICE("Streamed %s %s %s %d for %.0fs (%" PRIuMAX " frames)", opts.sensor_.c_str(), CamIdToStr(opts.camera_),
        CamDataTypeToStr(opts.data_), opts.rate_, t_str, boost::accumulators::count(stats.size));
    if (boost::accumulators::count(stats.lat) > 10) {  // clang-format off
        INFO("Latency receiving data [ms]: mean %4.1f (std %4.1f) min/0.5/0.68/0.95/0.997/max %4.1f %4.1f %4.1f %4.1f %4.1f %4.1f",  // clang-format on
            boost::accumulators::mean(stats.lat), std::sqrt(boost::accumulators::variance(stats.lat)),
            boost::accumulators::min(stats.lat), boost::accumulators::extended_p_square(stats.lat)[0],
            boost::accumulators::extended_p_square(stats.lat)[1], boost::accumulators::extended_p_square(stats.lat)[2],
            boost::accumulators::extended_p_square(stats.lat)[3], boost::accumulators::max(stats.lat));
    }
#if FPSDK_USE_FFMPEG
    if (boost::accumulators::count(stats.dec) > 10) {  // clang-format off
        INFO("Latency decoding video [ms]: mean %4.1f (std %4.1f) min/0.5/0.68/0.95/0.997/max %4.1f %4.1f %4.1f %4.1f %4.1f %4.1f",  // clang-format on
            boost::accumulators::mean(stats.dec), std::sqrt(boost::accumulators::variance(stats.dec)),
            boost::accumulators::min(stats.dec), boost::accumulators::extended_p_square(stats.dec)[0],
            boost::accumulators::extended_p_square(stats.dec)[1], boost::accumulators::extended_p_square(stats.dec)[2],
            boost::accumulators::extended_p_square(stats.dec)[3], boost::accumulators::max(stats.dec));
    }
#endif
    if (boost::accumulators::count(stats.exp) > 10) {  // clang-format off
        INFO("Exposure duration [ms]:      mean %4.1f (std %4.1f) min/0.5/0.68/0.95/0.997/max %4.1f %4.1f %4.1f %4.1f %4.1f %4.1f",  // clang-format on
            boost::accumulators::mean(stats.exp), std::sqrt(boost::accumulators::variance(stats.exp)),
            boost::accumulators::min(stats.exp), boost::accumulators::extended_p_square(stats.exp)[0],
            boost::accumulators::extended_p_square(stats.exp)[1], boost::accumulators::extended_p_square(stats.exp)[2],
            boost::accumulators::extended_p_square(stats.exp)[3], boost::accumulators::max(stats.exp));
    }
    if ((boost::accumulators::count(stats.size) > 10) && (t_str > 1.0)) {  // clang-format off
        INFO("Data size per frame [KiB]:   mean %4.0f (std %4.0f) min/0.5/0.68/0.95/0.997/max %4.0f %4.0f %4.0f %4.0f %4.0f %4.0f",  // clang-format on
            boost::accumulators::mean(stats.size), std::sqrt(boost::accumulators::variance(stats.size)),
            boost::accumulators::min(stats.size), boost::accumulators::extended_p_square(stats.size)[0],
            boost::accumulators::extended_p_square(stats.size)[1],
            boost::accumulators::extended_p_square(stats.size)[2],
            boost::accumulators::extended_p_square(stats.size)[3], boost::accumulators::max(stats.size));
        const double avg = boost::accumulators::sum(stats.size) / t_str;
        // Assuming GibE link TCP socket ~115MB/s = ~120500 KiB/s
        INFO("Average data rate [KiB/s]: %.0f (~%.1f%%GigE)", avg, avg / 120500.0 * 1e2);
    }
 
    return ok ? EXIT_SUCCESS : EXIT_FAILURE;
}
 
/* ****************************************************************************************************************** */