hight-level-robotics-congnitive-agent/librealsense/src/metadata-parser.h

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2024 Intel Corporation. All Rights Reserved.
#pragma once

// Metadata attributes provided by RS4xx Depth Cameras

#include "frame.h"
#include "metadata.h"

#include <cmath>
#include <rsutils/number/crc32.h>
#include <rsutils/string/from.h>
#include <rsutils/easylogging/easyloggingpp.h>


namespace librealsense
{
    /**\brief Base class that establishes the interface for retrieving metadata attributes*/
    class md_attribute_parser_base
    {
    public:
        virtual bool find( const frame & frm, rs2_metadata_type * p_value ) const = 0;

        virtual ~md_attribute_parser_base() = default;
    };

    /**\brief metadata parser class - support metadata in format: rs2_frame_metadata_value, rs2_metadata_type */
    class md_constant_parser : public md_attribute_parser_base
    {
    public:
        md_constant_parser(rs2_frame_metadata_value type) : _type(type) {}

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            const uint8_t * pos = frm.additional_data.metadata_blob.data();
            while( pos <= frm.additional_data.metadata_blob.data() + frm.additional_data.metadata_blob.size() )
            {
                const rs2_frame_metadata_value * type = reinterpret_cast<const rs2_frame_metadata_value *>(pos);
                pos += sizeof( rs2_frame_metadata_value );
                if( _type == *type )
                {
                    const rs2_metadata_type * value = reinterpret_cast<const rs2_metadata_type *>(pos);
                    if( p_value )
                        memcpy( (void *) p_value, (const void *) value, sizeof( *value ) );
                    return true;
                }
                pos += sizeof( rs2_metadata_type );
            }
            return false;
        }

        static std::shared_ptr<metadata_parser_map> create_metadata_parser_map()
        {
            auto md_parser_map = std::make_shared<metadata_parser_map>();
            for (int i = 0; i < static_cast<int>(rs2_frame_metadata_value::RS2_FRAME_METADATA_COUNT); ++i)
            {
                auto frame_md_type = static_cast<rs2_frame_metadata_value>(i);
                md_parser_map->insert(std::make_pair(frame_md_type, std::make_shared<md_constant_parser>(frame_md_type)));
            }
            return md_parser_map;
        }

    private:
        rs2_frame_metadata_value _type;
    };


    /**\brief metadata parser class - support metadata as array of (bool,rs2_metadata_type) */
    class md_array_parser : public md_attribute_parser_base
    {
        rs2_frame_metadata_value _key;

    public:
        md_array_parser( rs2_frame_metadata_value key )
            : _key( key )
        {
        }

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            auto pmd = reinterpret_cast< metadata_array_value const * >( frm.additional_data.metadata_blob.data() );
            metadata_array_value const & value = pmd[_key];
            if( ! value.is_valid )
                return false;
            if( p_value )
                *p_value = value.value;
            return true;
        }
    };


    /**\brief Post-processing adjustment of the metadata attribute
     *  e.g change auto_exposure enum to boolean, change units from nano->ms,etc'*/
    typedef std::function<rs2_metadata_type(const rs2_metadata_type& param)> attrib_modifyer;

    class md_time_of_arrival_parser : public md_attribute_parser_base
    {
    public:
        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            if( p_value )
                *p_value = (rs2_metadata_type)frm.get_frame_system_time();
            return true;
        }
    };

    /**\brief The metadata parser class directly access the metadata attribute in the blob received from HW.
    *   Given the metadata-nested construct, and the c++ lack of pointers
    *   to the inner struct, we pre-calculate and store the attribute offset internally
    *   http://stackoverflow.com/questions/1929887/is-pointer-to-inner-struct-member-forbidden
    */
    template<class S, class Attribute, typename Flag>
    class md_attribute_parser : public md_attribute_parser_base
    {
    public:
        md_attribute_parser( Attribute S::*attribute_name, Flag flag, unsigned long long offset, attrib_modifyer mod )
            : _md_attribute( attribute_name )
            , _md_flag( flag )
            , _offset( offset )
            , _modifyer( mod )
        {
        }

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            auto s = reinterpret_cast< const S * >( ( (const uint8_t *)frm.additional_data.metadata_blob.data() )
                                                    + _offset );

            if( ! is_attribute_valid( s ) )
                return false;

            if( p_value )
            {
                auto attrib = static_cast<rs2_metadata_type>((*s).*_md_attribute);
                if( _modifyer )
                    attrib = _modifyer( attrib );
                *p_value = attrib;
            }
            return true;
        }

    protected:
        bool is_attribute_valid( const S * s ) const
        {
            // verify that the struct is of the correct type
            // Check that the header id and the struct size corresponds.
            // Note that this heurisic is not deterministic and may validate false frames! TODO - requires review
            md_type expected_type = md_type_trait< S >::type;

            if( ( s->header.md_type_id != expected_type ) || ( s->header.md_size < sizeof( *s ) ) )
            {
                std::string type
                    = ( md_type_desc.count( s->header.md_type_id ) > 0 )
                        ? md_type_desc.at( s->header.md_type_id )
                        : ( rsutils::string::from()
                            << "0x" << std::hex << static_cast< uint32_t >( s->header.md_type_id ) << std::dec );
                LOG_DEBUG( "Metadata mismatch - actual: " << type << ", expected: 0x" << std::hex
                                                          << (uint32_t)expected_type << std::dec << " ("
                                                          << md_type_desc.at( expected_type ) << ")" );
                return false;
            }

            // Check if the attribute's flag is set
            auto attribute_enabled = ( 0 != ( s->flags & static_cast< uint32_t >( _md_flag ) ) );
            return attribute_enabled;
        }

    private:
        md_attribute_parser() = delete;
        md_attribute_parser(const md_attribute_parser&) = delete;

        Attribute S::*      _md_attribute;  // Pointer to the attribute within struct that holds the relevant data
        Flag                _md_flag;       // Bit that indicates whether the particular attribute is active
        unsigned long long  _offset;        // Inner struct offset with regard to the most outer one
        attrib_modifyer     _modifyer;      // Post-processing on received attribute
    };

    /**\brief A helper function to create a specialized attribute parser.
     *  Return it as a pointer to a base-class*/
    template<class S, class Attribute, typename Flag>
    std::shared_ptr<md_attribute_parser_base> make_attribute_parser(Attribute S::* attribute, Flag flag, unsigned long long offset, attrib_modifyer mod = nullptr)
    {
        std::shared_ptr<md_attribute_parser<S, Attribute, Flag>> parser(new md_attribute_parser<S, Attribute, Flag>(attribute, flag, offset, mod));
        return parser;
    }

    /**\brief A UVC-Header parser class*/
    template<class St, class Attribute>
    class md_uvc_header_parser : public md_attribute_parser_base
    {
    public:
        md_uvc_header_parser(Attribute St::* attribute_name, attrib_modifyer mod) :
            _md_attribute(attribute_name), _modifyer(mod){};

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            if( frm.additional_data.metadata_size < platform::uvc_header_size )
                return false;

            if( p_value )
            {
                auto attrib = static_cast< rs2_metadata_type >(
                    ( *reinterpret_cast< const St * >( (const uint8_t *)frm.additional_data.metadata_blob.data() ) )
                    .*_md_attribute );
                if( _modifyer )
                    attrib = _modifyer( attrib );
                *p_value = attrib;
            }
            return true;
        }

    private:
        md_uvc_header_parser() = delete;
        md_uvc_header_parser(const md_uvc_header_parser&) = delete;

        Attribute St::*     _md_attribute;  // Pointer to the attribute within uvc header that provides the relevant data
        attrib_modifyer     _modifyer;      // Post-processing on received attribute
    };

    /**\brief A utility function to create UVC metadata header parser*/
    template<class St, class Attribute>
    std::shared_ptr<md_attribute_parser_base> make_uvc_header_parser(Attribute St::* attribute, attrib_modifyer mod = nullptr)
    {
        std::shared_ptr<md_uvc_header_parser<St, Attribute>> parser(new md_uvc_header_parser<St, Attribute>(attribute, mod));
        return parser;
    }

    /**\brief A HID-Header metadata parser class*/
    template<class St, class Attribute>
    class md_hid_header_parser : public md_attribute_parser_base
    {
    public:
        md_hid_header_parser(Attribute St::* attribute_name, attrib_modifyer mod) :
            _md_attribute(attribute_name), _modifyer(mod) {};

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            if( frm.additional_data.metadata_size < hid_header_size )
                return false;

            if( p_value )
            {
                auto attrib = static_cast< rs2_metadata_type >(
                    ( *reinterpret_cast< const St * >( (const uint8_t *)frm.additional_data.metadata_blob.data() ) )
                    .*_md_attribute );
                attrib &= 0x00000000ffffffff;
                if( _modifyer )
                    attrib = _modifyer( attrib );
                *p_value = attrib;
            }
            return true;
        }

    private:
        md_hid_header_parser() = delete;
        md_hid_header_parser(const md_hid_header_parser&) = delete;

        Attribute St::*     _md_attribute;  // Pointer to the attribute within uvc header that provides the relevant data
        attrib_modifyer     _modifyer;      // Post-processing on received attribute
    };

    /**\brief A utility function to create HID metadata header parser*/
    template<class St, class Attribute>
    std::shared_ptr<md_attribute_parser_base> make_hid_header_parser(Attribute St::* attribute, attrib_modifyer mod = nullptr)
    {
        std::shared_ptr<md_hid_header_parser<St, Attribute>> parser(new md_hid_header_parser<St, Attribute>(attribute, mod));
        return parser;
    }

    /**\brief provide attributes generated and stored internally by the library*/
    template<class St, class Attribute>
    class md_additional_parser : public md_attribute_parser_base
    {
    public:
        md_additional_parser(Attribute St::* attribute_name) :
            _md_attribute(attribute_name) {};

        bool find( const frame & frm, rs2_metadata_type * p_value ) const override
        {
            if( p_value )
                *p_value = static_cast< rs2_metadata_type >( frm.additional_data.*_md_attribute );
            return true;
        }

    private:
        md_additional_parser() = delete;
        md_additional_parser(const md_additional_parser&) = delete;

        Attribute St::*          _md_attribute;      // Pointer to the attribute within uvc header that provides the relevant data
    };

    /**\brief A utility function to create additional_data parser*/
    template<class St, class Attribute>
    std::shared_ptr<md_attribute_parser_base> make_additional_data_parser(Attribute St::* attribute)
    {
        std::shared_ptr<md_additional_parser<St, Attribute>> parser(new md_additional_parser<St, Attribute>(attribute));
        return parser;
    }

    /**\brief Optical timestamp for RS4xx devices is calculated internally*/
    class md_rs400_sensor_timestamp : public md_attribute_parser_base
    {
        std::shared_ptr<md_attribute_parser_base> _sensor_ts_parser = nullptr;
        std::shared_ptr<md_attribute_parser_base> _frame_ts_parser = nullptr;

    public:
        explicit md_rs400_sensor_timestamp(std::shared_ptr<md_attribute_parser_base> sensor_ts_parser,
            std::shared_ptr<md_attribute_parser_base> frame_ts_parser) :
            _sensor_ts_parser(sensor_ts_parser), _frame_ts_parser(frame_ts_parser) {};

        virtual ~md_rs400_sensor_timestamp() { _sensor_ts_parser = nullptr; _frame_ts_parser = nullptr; };

        // The sensor's timestamp is defined as the middle of exposure time. Sensor_ts= Frame_ts - (Actual_Exposure/2)
        // For RS4xx the metadata payload holds only the (Actual_Exposure/2) offset, and the actual value needs to be calculated
        bool find( const librealsense::frame & frm, rs2_metadata_type * p_value ) const override
        {
            rs2_metadata_type frame_value, sensor_value;
            if( ! _sensor_ts_parser->find( frm, &sensor_value ) || ! _frame_ts_parser->find( frm, &frame_value ) )
                return false;
            if( p_value )
                *p_value = frame_value - sensor_value;
            return true;
        }
    };

    template<class S, class Attribute, typename Flag>
    class md_attribute_parser_with_crc : public md_attribute_parser<S, Attribute, Flag>
    {
    public: 
        md_attribute_parser_with_crc(Attribute S::* attribute_name, Flag flag, unsigned long long offset, attrib_modifyer mod, unsigned long long start_offset, unsigned long long crc_offset)
            : md_attribute_parser<S, Attribute, Flag>(attribute_name, flag, offset, mod), _start_offset(start_offset), _crc_offset(crc_offset) {}

    protected:
        unsigned long long _start_offset; // offset of the first field that is relevant for CRC calculation
        unsigned long long _crc_offset; // offset of the crc inside the S class
        bool is_attribute_valid(const S* s) const override
        {
            if (!md_attribute_parser<S, Attribute, Flag>::is_attribute_valid(s))
                return false;

            if (!is_crc_valid(s))
            {
                LOG_DEBUG("Metadata CRC mismatch");
                return false;
            }

            return true;
        }

    private:
        md_attribute_parser_with_crc() = delete;
        md_attribute_parser_with_crc(const md_attribute_parser_with_crc&) = delete;

        bool is_crc_valid(const S* md_info) const
        {
            // calculation of CRC32 for the MD payload, starting from "start_offset" till the end of struct, not including the CRC itself.
            // assuming the CRC field is the last field of the struct
            Attribute crc = *(Attribute*)(reinterpret_cast<const uint8_t*>(md_info) + _crc_offset);   //Attribute = CRC's type (ex. uint32_t)
            auto computed_crc32 = rsutils::number::calc_crc32(reinterpret_cast<const uint8_t*>(md_info) + _start_offset,
                sizeof(S) - _start_offset - sizeof(crc));
            return (crc == computed_crc32);
        }
    };

    /**\brief A helper function to create a specialized attribute parser.
     *  **Note that this class is assuming that the CRC is the last variable in the struct**
     *  Return it as a pointer to a base-class*/
    template<class S, class Attribute, typename Flag>
    std::shared_ptr<md_attribute_parser_base> make_attribute_parser_with_crc(Attribute S::* attribute, Flag flag, unsigned long long offset, unsigned long long start_offset, unsigned long long crc_offset, attrib_modifyer mod = nullptr)
    {
        std::shared_ptr<md_attribute_parser<S, Attribute, Flag>> parser(new md_attribute_parser_with_crc<S, Attribute, Flag>(attribute, flag, offset, mod, start_offset, crc_offset));
        return parser;
    }



    class ds_md_attribute_actual_fps : public md_attribute_parser_base
    {
    public:
        bool find( const librealsense::frame & frm, rs2_metadata_type * p_value ) const override
        {
            auto fps = rs2_metadata_type( frm.calc_actual_fps() * 1000 );
            if( fps <= 0. )
                // In case of frame counter reset fallback to fps from the stream configuration
                return false;
            if( p_value )
                *p_value = fps;
            return true;
        }
    };

    /**\brief A helper function to create a specialized parser for RS4xx sensor timestamp*/
    inline std::shared_ptr<md_attribute_parser_base> make_rs400_sensor_ts_parser(std::shared_ptr<md_attribute_parser_base> frame_ts_parser,
        std::shared_ptr<md_attribute_parser_base> sensor_ts_parser)
    {
        std::shared_ptr<md_rs400_sensor_timestamp> parser(new md_rs400_sensor_timestamp(sensor_ts_parser, frame_ts_parser));
        return parser;
    }
}