VoxlHK.h

Go to the documentation of this file.

/**
 * @file VoxlHK.h
 * @brief Housekeeping and data publishing for VOXL OpenVINS
 * @author Joao Leonardo Silva Cotta (@zauberflote1)
 * @date 2025
 * @version 1.0
 *
 * This header defines the housekeeping system for the VOXL OpenVINS server,
 * including data publishing, angular velocity calculations, and coordinate
 * frame transformations. It provides the interface for outputting VIO data
 * to external systems.
 */
 
#ifndef VOXL_HK_H
#define VOXL_HK_H
#pragma once
 
// Standard includes
#include <atomic>
#include <memory>
 
// Third-party includes
#include <modal_pipe.h>
#include <modal_json.h>
#include <voxl_common_config.h>
#include <core/VioManager.h>
#include <core/VioManagerOptions.h>
#include <types/LandmarkRepresentation.h>
#include <cstring>
#include <state/State.h>
#include <state/StateHelper.h>
#include <types/Landmark.h>
#include <types/Type.h>
#include <feat/Feature.h>
#include <cmath>
#include <algorithm>
#include <thread>
#include <chrono>
#include <Eigen/Eigenvalues>
// Local includes
#include "VoxlVars.h"
#include "VoxlCommon.h"
 
namespace voxl
{
 
    // ============================================================================
    // UTILITY FUNCTIONS
    // ============================================================================
 
    /**
     * @brief Calculate angular velocity from consecutive quaternions
     *
     * Computes angular velocity via axis-angle extraction from the relative
     * rotation dq = q1 * inv(q0). Falls back to a small-angle approximation
     * when the rotation is near-identity (degenerate case).
     *
     * @param q0 Previous quaternion (4x1, JPL convention)
     * @param q1 Current  quaternion (4x1, JPL convention)
     * @param dt Time difference in **nanoseconds**
     * @return Angular velocity vector (3x1) in rad/s
     */
    inline Eigen::Matrix<double, 3, 1> dirtyOmega(const Eigen::Matrix<double, 4, 1> &q0,
                                                   const Eigen::Matrix<double, 4, 1> &q1,
                                                   double dt)
    {
        // Convert nanoseconds to seconds
        dt *= 1e-9;
 
        if (dt <= 0.0) return Eigen::Vector3d::Zero();
        if (q0.isZero(0) || q1.isZero(0)) return Eigen::Vector3d::Zero();
 
        // Relative rotation: dq = q1 * inv(q0)
        Eigen::Matrix<double, 4, 1> dq = ov_core::quat_multiply(q1, ov_core::Inv(q0));
        dq /= dq.norm();
 
        // Shortest path
        if (dq.w() < 0.0)
            dq = -dq;
 
        double w = std::clamp(dq.w(), -1.0, 1.0);
        double sin_half = std::sqrt(std::max(1.0 - w * w, 0.0));
 
        // Degenerate (small-angle) case: fall back to simple approximation
        if (sin_half < 1e-8)
            return (2.0 / dt) * dq.head<3>();
 
        // General case: proper axis-angle extraction
        double theta = 2.0 * std::acos(w);
        Eigen::Vector3d axis = dq.head<3>() / sin_half;
        return axis * (theta / dt);
    }
 
    /**
     * @brief Get OpenVINS to FRD coordinate frame transformation matrix
     *
     * This function returns the rotation matrix that transforms from the
     * OpenVINS coordinate frame to the Front-Right-Down (FRD) coordinate frame
     * commonly used in aerospace applications.
     *
     * TODO: Port this for greater possibility of orientations --> case by case etc
     *
     * @return 3x3 rotation matrix from OpenVINS to FRD frame
     */
    inline const Eigen::Matrix3d &R_OV_FRD()
    {
        static const Eigen::Matrix3d R{
            (Eigen::Matrix3d() << 1, 0, 0, 0, -1, 0, 0, 0, -1).finished()};
        return R;
    }
 
    // ============================================================================
    // PUBLISHER CLASS
    // ============================================================================
 
    /**
     * @class Publisher
     * @brief Singleton class for publishing VIO data
     *
     * This class manages the publication of VIO data to external systems through
     * pipe interfaces. It implements the singleton pattern to ensure only one
     * instance exists throughout the application lifecycle.
     *
     * The publisher handles:
     * - VIO state data formatting
     * - Track base information
     * - Coordinate frame transformations
     * - Data packet generation and transmission
     */
    class Publisher
    {
    public:
        /**
         * @brief Get singleton instance
         * @return Reference to the singleton Publisher instance
         */
        static Publisher &getInstance()
        {
            static Publisher instance;
            return instance;
        }
 
        // Delete copy constructor and assignment operator for singleton
        Publisher(const Publisher &) = delete;
        Publisher &operator=(const Publisher &) = delete;
 
        /**
         * @brief Start the publisher
         *
         * Initializes the publisher and prepares it for data transmission.
         */
        void start();
 
        /**
         * @brief Control pipe callback function
         *
         * Callback function to handle control pipe messages
         */
        static void ov_vio_control_pipe_cb(int ch, char *string, int bytes, void *context);
 
        /**
         * @brief Publish VIO data
         *
         * Publishes the current VIO state and tracking information to external
         * systems through the configured pipe interfaces.
         *
         * @param state Current VIO state
         * @param used_features_map Used-features map, by reference (no copy). The caller runs on
         *        the VIO thread synchronously after the update, so the reference stays valid for
         *        the whole call; publish must never re-enter feed/drain.
         */
        void publish(std::shared_ptr<ov_msckf::State> state,
                     const std::map<double, std::vector<std::shared_ptr<ov_core::Feature>>> &used_features_map = {});
 
        /**
         * @brief Stop the publisher
         *
         * Stops the publisher and cleans up resources.
         */
        void stop();
 
        /**
         * @brief Check if auto-reset should be triggered
         *
         * Evaluates current VIO state and error conditions to determine
         * if an automatic reset should be triggered.
         *
         * @param state Current VIO state
         * @param quality Current quality value
         * @param n_features Number of tracked features
         * @param yawrate Calculated yaw rate from angular velocity
         * @param current_velocity Current velocity magnitude
         * @param vel_x X-component of velocity
         * @param vel_y Y-component of velocity
         * @return true if auto-reset should be triggered, false otherwise
         */
        bool should_auto_reset(std::shared_ptr<ov_msckf::State> state,
                               int quality,
                               int n_features,
                               double yawrate,
                               double current_velocity,
                               double vel_x,
                               double vel_y);
 
        /**
         * @brief Calculate Quality of the VIO state (async-aware per-camera freshness ledger)
         *
         * Pools all used-feature entries from the last QUAL_UNION_WINDOW_S (dedup by featid) and
         * scores each camera over the pooled set with the unchanged synced-path grid metric
         * (5x5 grid, SLAM features weighted by covariance largest eigenvalue + quality field,
         * MSCKF by quality field + track length), sum-then-clamp as sync always fused. The union
         * gives each camera its full recent track set -- one update's used list is a sparse,
         * noisy sample -- so async ticks score like synced batches; a time-constant EMA smooths
         * the published value against single weak updates. Floor is 10 while running (the metric
         * alone must not trip the sustained quality<1 auto-reset timers); resets/FAILED force
         * 0/-1 in publish() itself.
         *
         * Must only be called from the VIO thread (publish); uses the qual_ema state with no
         * locks by design.
         *
         * @param used_features_map Map of used features keyed by update (epoch) timestamp
         * @param slam_features Map of SLAM features from the state
         * @param state Current VIO state for covariance access
         * @return Quality score (10-100 while running, higher is better)
         */
        double calcQuality(const std::map<double, std::vector<std::shared_ptr<ov_core::Feature>>> &used_features_map,
                           std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> &slam_features,
                           std::shared_ptr<ov_msckf::State> state);
 
        /**
         * @brief Set the first packet flag
         *
         * This function sets the first packet flag to the provided value.
         *
         * @param first_packet The value to set the first packet flag to
         */
        void set_first_packet(bool first_packet_)
        {
            first_packet = first_packet_;
        };
        // ADD BLANK PUBLISHER TO INDICATE MISSING SENSORS
        void publishBlank();
 
    private:
        /**
         * @brief Private constructor for singleton pattern
         */
        Publisher();
 
        /**
         * @brief Private destructor for singleton pattern
         */
        ~Publisher();
 
        // ============================================================================
        // PRIVATE MEMBER VARIABLES
        // ============================================================================
 
        /** @brief Flag indicating if this is the first packet */
        bool first_packet = true;
 
        /** @brief Tracks the rising edge of vio_manager->initialized() so the
         *  origin snapshot is re-captured on every (re)init, including in-flight
         *  resets where the Publisher singleton persists. */
        bool prev_initialized = false;
 
        /** @brief Yaw-only origin rotation captured at each init. Zeros the
         *  (unobservable) heading at start while preserving gravity-true
         *  roll/pitch. Identity until the first init. */
        Eigen::Matrix3d ned_rot_zero = Eigen::Matrix3d::Identity();
 
        /** @brief VIO data packet structure */
        vio_data_t vio_packet;
 
        /** @brief Previous quaternion for angular velocity calculation */
        Eigen::Matrix<double, 4, 1> past_q_I_G;
 
        /** @brief State for the async-aware quality metric (see calcQuality). Written only
         *  inside publish() on the VIO/ingest thread -- single-writer, single-reader, no locks
         *  by design. Invalidated by publish() on the same condition that resets the quality
         *  hysteresis machine (reset counter change / FAILED), which fires for BOTH hard and
         *  soft resets, so stale pre-reset values can never pre-charge a new episode (sensor
         *  time is continuous across resets -- timestamps alone cannot tell). */
        static constexpr int QUAL_MAX_CAMS = 4;
        double qual_ema = -1.0;      ///< smoothed fused quality; <0 = unseeded/invalid
        double qual_ema_t = 0.0;     ///< sensor time of the last EMA update
        double qual_src_key = 0.0;   ///< newest map key last scored (rescore fingerprint)
        size_t qual_src_size = 0;    ///< size of that entry when scored (appends re-dirty it)
        size_t qual_src_map_size = 0; ///< map size when scored
    };
 
    // ============================================================================
    // HEALTH CHECK CLASS
    // ============================================================================
 
    /**
     * @class HealthCheck
     * @brief Health monitoring system for VOXL OpenVINS
     *
     * This class provides comprehensive health monitoring capabilities for
     * the VIO system, including error code monitoring, system state checks,
     * and performance monitoring. It runs at 30Hz and continuously monitors
     * the system health.
     */
    class HealthCheck
    {
    public:
        /**
         * @brief Get singleton instance
         * @return Reference to the singleton HealthCheck instance
         */
        static HealthCheck &getInstance()
        {
            static HealthCheck instance;
            return instance;
        }
 
        // Delete copy constructor and assignment operator for singleton
        HealthCheck(const HealthCheck &) = delete;
        HealthCheck &operator=(const HealthCheck &) = delete;
 
        /**
         * @brief Start the health check system
         *
         * Initializes and starts the health monitoring thread that runs at 30Hz.
         * The thread continuously monitors system health and error conditions.
         */
        void start();
 
        /**
         * @brief Stop the health check system
         *
         * Stops the health monitoring thread and performs cleanup.
         */
        void stop();
 
        /**
         * @brief Check if health monitoring is running
         * @return true if health check is active, false otherwise
         */
        bool isRunning() const { return running_.load(); }
 
        /**
         * @brief Clear specific error codes
         *
         * Clears the specified error codes from the global error state.
         * This is useful when errors are resolved and should no longer
         * be reported.
         *
         * @param error_mask Bit mask of error codes to clear
         * @param clear_all If true, clear all error codes
         */
        static void clearErrorCodes(uint32_t error_mask)
        {
            clearErrorCodes(error_mask, false);
        }
        static void clearErrorCodes(uint32_t error_mask, bool clear_all);
 
    private:
        /**
         * @brief Private constructor for singleton pattern
         */
        HealthCheck();
 
        /**
         * @brief Private destructor for singleton pattern
         */
        ~HealthCheck();
 
        /**
         * @brief Main health check loop
         *
         * Runs at 30Hz and performs comprehensive health monitoring including:
         * - Error code analysis and logging
         * - System state validation
         * - Performance monitoring
         * - Auto-reset condition checking
         */
        void healthCheckLoop();
 
        /**
         * @brief Analyze and log error codes
         *
         * Examines the current error codes and logs detailed information
         * about any active errors or warnings.
         */
        void analyzeErrorCodes();
 
        /**
         * @brief Check system connectivity
         *
         * Monitors the connection status of cameras and IMU, logging
         * any disconnection events or connectivity issues.
         */
        void checkSystemConnectivity();
 
        /**
         * @brief Monitor system performance
         *
         * Tracks system performance metrics including processing rates,
         * memory usage, and timing statistics.
         */
        void monitorSystemPerformance();
 
        /**
         * @brief Check auto-reset conditions
         *
         * Evaluates whether auto-reset conditions are met based on
         * current system state and error conditions.
         */
        void checkAutoResetConditions();
 
        /**
         * @brief Handle VINS reset command
         *
         * This function is invoked when a reset command is received.
         * It performs the necessary actions to reset the VIO system.
         */
        void checkVINSResetRequest();
 
        /**
         * @brief Perform a hard reset of the VIO system
         *
         * This function creates a fresh instance of the
         * VIO manager and reinitializes the system.,
         */
        int doHardReset();
 
        /**
         * @brief Perform a front-end-preserving SOFT reset.
         *
         * Resets the navigation EKF via VioManager::soft_reset() while keeping the feature DB and
         * IMU history so re-init fires fast (no full-window re-collection). Escalates to
         * doHardReset() on failure or if there is no live VIO manager.
         */
        int doSoftReset();
 
        // ============================================================================
        // PRIVATE MEMBER VARIABLES
        // ============================================================================
 
        /** @brief Flag indicating if health check is running */
        std::atomic<bool> running_{false};
 
        /** @brief Health check thread */
        std::thread health_thread_;
 
        /** @brief Last error code state for change detection */
        uint32_t last_error_codes_{0};
 
        /** @brief Last VIO state for change detection */
        uint8_t last_vio_state_{0};
 
        /** @brief Last IMU connection state for change detection */
        bool last_imu_connected_{false};
 
        /** @brief Last camera connection state for change detection */
        bool last_cam_connected_{false};
 
        /** @brief Used to track the first camera connection */
        bool first_camera_connection_seen_{false};
 
        /** @brief Timestamp of last health check */
        int64_t last_health_check_ns_{0};
 
        /** @brief Counter for health check iterations */
        uint64_t health_check_count_{0};
 
        /** @brief Mutex for thread safety */
        mutable std::mutex health_mutex_;
 
        /** @brief Timeout interval before we can reset again */
        const uint64_t INIT_FAILURE_TIMEOUT_NS = 2000000000; // 2 seconds
 
        /** @brief Timestamp of last reset */
        uint64_t time_of_last_reset = 0;
    };
 
} // namespace voxl
 
#endif // VOXL_HK_H