rs_sensor.hpp

Go to the documentation of this file.

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

#ifndef LIBREALSENSE_RS2_SENSOR_HPP
#define LIBREALSENSE_RS2_SENSOR_HPP

#include "rs_types.hpp"
#include "rs_frame.hpp"
#include "rs_processing.hpp"
#include "rs_options.hpp"
namespace rs2
{

    class notification
    {
    public:
        notification(rs2_notification* nt)
        {
            rs2_error* e = nullptr;
            _description = rs2_get_notification_description(nt, &e);
            error::handle(e);
            _timestamp = rs2_get_notification_timestamp(nt, &e);
            error::handle(e);
            _severity = rs2_get_notification_severity(nt, &e);
            error::handle(e);
            _category = rs2_get_notification_category(nt, &e);
            error::handle(e);
            _serialized_data = rs2_get_notification_serialized_data(nt, &e);
            error::handle(e);
        }

        notification() = default;

        rs2_notification_category get_category() const
        {
            return _category;
        }
        std::string get_description() const
        {
            return _description;
        }

        double get_timestamp() const
        {
            return _timestamp;
        }

        rs2_log_severity get_severity() const
        {
            return _severity;
        }

        std::string get_serialized_data() const
        {
            return _serialized_data;
        }

    private:
        std::string _description;
        double _timestamp = -1;
        rs2_log_severity _severity = RS2_LOG_SEVERITY_COUNT;
        rs2_notification_category _category = RS2_NOTIFICATION_CATEGORY_COUNT;
        std::string _serialized_data;
    };

    template<class T>
    class notifications_callback : public rs2_notifications_callback
    {
        T on_notification_function;
    public:
        explicit notifications_callback(T on_notification) : on_notification_function(on_notification) {}

        void on_notification(rs2_notification* _notification) override
        {
            on_notification_function(notification{ _notification });
        }

        void release() override { delete this; }
    };


    class sensor : public options
    {
    public:

        using options::supports;
        void open(const stream_profile& profile) const
        {
            rs2_error* e = nullptr;
            rs2_open(_sensor.get(),
                profile.get(),
                &e);
            error::handle(e);
        }

        bool supports(rs2_camera_info info) const
        {
            rs2_error* e = nullptr;
            auto is_supported = rs2_supports_sensor_info(_sensor.get(), info, &e);
            error::handle(e);
            return is_supported > 0;
        }

        const char* get_info(rs2_camera_info info) const
        {
            rs2_error* e = nullptr;
            auto result = rs2_get_sensor_info(_sensor.get(), info, &e);
            error::handle(e);
            return result;
        }

        void open(const std::vector<stream_profile>& profiles) const
        {
            rs2_error* e = nullptr;

            std::vector<const rs2_stream_profile*> profs;
            profs.reserve(profiles.size());
            for (auto& p : profiles)
            {
                profs.push_back(p.get());
            }

            rs2_open_multiple(_sensor.get(),
                profs.data(),
                static_cast<int>(profiles.size()),
                &e);
            error::handle(e);
        }

        void close() const
        {
            rs2_error* e = nullptr;
            rs2_close(_sensor.get(), &e);
            error::handle(e);
        }

        template<class T>
        void start(T callback) const
        {
            rs2_error* e = nullptr;
            rs2_start_cpp(_sensor.get(), new frame_callback<T>(std::move(callback)), &e);
            error::handle(e);
        }

        void stop() const
        {
            rs2_error* e = nullptr;
            rs2_stop(_sensor.get(), &e);
            error::handle(e);
        }

        template<class T>
        void set_notifications_callback(T callback) const
        {
            rs2_error* e = nullptr;
            rs2_set_notifications_callback_cpp(_sensor.get(),
                new notifications_callback<T>(std::move(callback)), &e);
            error::handle(e);
        }

        std::vector<stream_profile> get_stream_profiles() const
        {
            std::vector<stream_profile> results{};

            rs2_error* e = nullptr;
            std::shared_ptr<rs2_stream_profile_list> list(
                rs2_get_stream_profiles(_sensor.get(), &e),
                rs2_delete_stream_profiles_list);
            error::handle(e);

            auto size = rs2_get_stream_profiles_count(list.get(), &e);
            error::handle(e);

            for (auto i = 0; i < size; i++)
            {
                stream_profile profile(rs2_get_stream_profile(list.get(), i, &e));
                error::handle(e);
                results.push_back(profile);
            }

            return results;
        }

        std::vector<stream_profile> get_active_streams() const
        {
            std::vector<stream_profile> results{};

            rs2_error* e = nullptr;
            std::shared_ptr<rs2_stream_profile_list> list(
                rs2_get_active_streams(_sensor.get(), &e),
                rs2_delete_stream_profiles_list);
            error::handle(e);

            auto size = rs2_get_stream_profiles_count(list.get(), &e);
            error::handle(e);

            for (auto i = 0; i < size; i++)
            {
                stream_profile profile(rs2_get_stream_profile(list.get(), i, &e));
                error::handle(e);
                results.push_back(profile);
            }

            return results;
        }

        std::vector<filter> get_recommended_filters() const
        {
            std::vector<filter> results{};

            rs2_error* e = nullptr;
            std::shared_ptr<rs2_processing_block_list> list(
                rs2_get_recommended_processing_blocks(_sensor.get(), &e),
                rs2_delete_recommended_processing_blocks);
            error::handle(e);

            auto size =  rs2_get_recommended_processing_blocks_count(list.get(), &e);
            error::handle(e);

            for (auto i = 0; i < size; i++)
            {
                auto f = std::shared_ptr<rs2_processing_block>(
                    rs2_get_processing_block(list.get(), i, &e),
                    rs2_delete_processing_block);
                error::handle(e);
                results.push_back(f);
            }

            return results;
        }

        sensor& operator=(const std::shared_ptr<rs2_sensor> other)
        {
            options::operator=(other);
            _sensor.reset();
            _sensor = other;
            return *this;
        }

        sensor& operator=(const sensor& other)
        {
            *this = nullptr;
             options::operator=(other._sensor);
            _sensor = other._sensor;
            return *this;
        }
        sensor() : _sensor(nullptr) {}

        operator bool() const
        {
            return _sensor != nullptr;
        }

        const std::shared_ptr<rs2_sensor>& get() const
        {
            return _sensor;
        }

        template<class T>
        bool is() const
        {
            T extension(*this);
            return extension;
        }

        template<class T>
        T as() const
        {
            T extension(*this);
            return extension;
        }

        explicit sensor(std::shared_ptr<rs2_sensor> dev)
            :options((rs2_options*)dev.get()), _sensor(dev)
        {
        }
        explicit operator std::shared_ptr<rs2_sensor>() { return _sensor; }

    protected:
        friend context;
        friend device_list;
        friend device;
        friend device_base;
        friend roi_sensor;

        std::shared_ptr<rs2_sensor> _sensor;


    };

    inline std::shared_ptr<sensor> sensor_from_frame(frame f)
    {
        std::shared_ptr<rs2_sensor> psens(f.get_sensor(), rs2_delete_sensor);
        return std::make_shared<sensor>(psens);
    }

    inline bool operator==(const sensor& lhs, const sensor& rhs)
    {
        if (!(lhs.supports(RS2_CAMERA_INFO_NAME) && lhs.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)
            && rhs.supports(RS2_CAMERA_INFO_NAME) && rhs.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)))
            return false;

        return std::string(lhs.get_info(RS2_CAMERA_INFO_NAME)) == rhs.get_info(RS2_CAMERA_INFO_NAME)
            && std::string(lhs.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER)) == rhs.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
    }

    class color_sensor : public sensor
    {
    public:
        color_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_COLOR_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }
        operator bool() const { return _sensor.get() != nullptr; }
    };

    class motion_sensor : public sensor
    {
    public:
        motion_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_MOTION_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }
        operator bool() const { return _sensor.get() != nullptr; }
    };

    class fisheye_sensor : public sensor
    {
    public:
        fisheye_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_FISHEYE_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }
        operator bool() const { return _sensor.get() != nullptr; }
    };

    class roi_sensor : public sensor
    {
    public:
        roi_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if(rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_ROI, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        void set_region_of_interest(const region_of_interest& roi)
        {
            rs2_error* e = nullptr;
            rs2_set_region_of_interest(_sensor.get(), roi.min_x, roi.min_y, roi.max_x, roi.max_y, &e);
            error::handle(e);
        }

        region_of_interest get_region_of_interest() const
        {
            region_of_interest roi {};
            rs2_error* e = nullptr;
            rs2_get_region_of_interest(_sensor.get(), &roi.min_x, &roi.min_y, &roi.max_x, &roi.max_y, &e);
            error::handle(e);
            return roi;
        }

        operator bool() const { return _sensor.get() != nullptr; }
    };

    class depth_sensor : public sensor
    {
    public:
        depth_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_DEPTH_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        float get_depth_scale() const
        {
            rs2_error* e = nullptr;
            auto res = rs2_get_depth_scale(_sensor.get(), &e);
            error::handle(e);
            return res;
        }

        operator bool() const { return _sensor.get() != nullptr; }
        explicit depth_sensor(std::shared_ptr<rs2_sensor> dev) : depth_sensor(sensor(dev)) {}
    };

    class depth_stereo_sensor : public depth_sensor
    {
    public:
        depth_stereo_sensor(sensor s): depth_sensor(s)
        {
            rs2_error* e = nullptr;
            if (_sensor && rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_DEPTH_STEREO_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        float get_stereo_baseline() const
        {
            rs2_error* e = nullptr;
            auto res = rs2_get_stereo_baseline(_sensor.get(), &e);
            error::handle(e);
            return res;
        }

        operator bool() const { return _sensor.get() != nullptr; }
    };


    class pose_sensor : public sensor
    {
    public:
        pose_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_POSE_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        bool import_localization_map(const std::vector<uint8_t>& lmap_buf) const
        {
            rs2_error* e = nullptr;
            auto res = rs2_import_localization_map(_sensor.get(), lmap_buf.data(), uint32_t(lmap_buf.size()), &e);
            error::handle(e);
            return !!res;
        }

        std::vector<uint8_t> export_localization_map() const
        {
            std::vector<uint8_t> results;
            rs2_error* e = nullptr;
            const rs2_raw_data_buffer *map = rs2_export_localization_map(_sensor.get(), &e);
            error::handle(e);
            std::shared_ptr<const rs2_raw_data_buffer> loc_map(map, rs2_delete_raw_data);

            auto start = rs2_get_raw_data(loc_map.get(), &e);
            error::handle(e);

            if (start)
            {
                auto size = rs2_get_raw_data_size(loc_map.get(), &e);
                error::handle(e);

                results = std::vector<uint8_t>(start, start + size);
            }
            return results;
        }

        bool set_static_node(const std::string& guid, const rs2_vector& pos, const rs2_quaternion& orient) const
        {
            rs2_error* e = nullptr;
            auto res = rs2_set_static_node(_sensor.get(), guid.c_str(), pos, orient, &e);
            error::handle(e);
            return !!res;
        }


        bool get_static_node(const std::string& guid, rs2_vector& pos, rs2_quaternion& orient) const
        {
            rs2_error* e = nullptr;
            auto res = rs2_get_static_node(_sensor.get(), guid.c_str(), &pos, &orient, &e);
            error::handle(e);
            return !!res;
        }

        bool remove_static_node(const std::string& guid) const
        {
            rs2_error* e = nullptr;
            auto res = rs2_remove_static_node(_sensor.get(), guid.c_str(), &e);
            error::handle(e);
            return !!res;
        }

        operator bool() const { return _sensor.get() != nullptr; }
        explicit pose_sensor(std::shared_ptr<rs2_sensor> dev) : pose_sensor(sensor(dev)) {}
    };

    class wheel_odometer : public sensor
    {
    public:
        wheel_odometer(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_WHEEL_ODOMETER, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        bool load_wheel_odometery_config(const std::vector<uint8_t>& odometry_config_buf) const
        {
            rs2_error* e = nullptr;
            auto res = rs2_load_wheel_odometry_config(_sensor.get(), odometry_config_buf.data(), uint32_t(odometry_config_buf.size()), &e);
            error::handle(e);
            return !!res;
        }

        bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector& translational_velocity)
        {
            rs2_error* e = nullptr;
            auto res = rs2_send_wheel_odometry(_sensor.get(), wo_sensor_id, frame_num, translational_velocity, &e);
            error::handle(e);
            return !!res;
        }

        operator bool() const { return _sensor.get() != nullptr; }
        explicit wheel_odometer(std::shared_ptr<rs2_sensor> dev) : wheel_odometer(sensor(dev)) {}
    };

    class calibrated_sensor : public sensor
    {
    public:
        calibrated_sensor( sensor s )
            : sensor( s.get() )
        {
            rs2_error* e = nullptr;
            if( rs2_is_sensor_extendable_to( _sensor.get(), RS2_EXTENSION_CALIBRATED_SENSOR, &e ) == 0 && !e )
            {
                _sensor.reset();
            }
            error::handle( e );
        }

        operator bool() const { return _sensor.get() != nullptr; }

        void override_intrinsics( rs2_intrinsics const& intr )
        {
            rs2_error* e = nullptr;
            rs2_override_intrinsics( _sensor.get(), &intr, &e );
            error::handle( e );
        }

        void override_extrinsics( rs2_extrinsics const& extr )
        {
            rs2_error* e = nullptr;
            rs2_override_extrinsics( _sensor.get(), &extr, &e );
            error::handle( e );
        }

        rs2_dsm_params get_dsm_params() const
        {
            rs2_error* e = nullptr;
            rs2_dsm_params params;
            rs2_get_dsm_params( _sensor.get(), &params, &e );
            error::handle( e );
            return params;
        }

        void override_dsm_params( rs2_dsm_params const & params )
        {
            rs2_error* e = nullptr;
            rs2_override_dsm_params( _sensor.get(), &params, &e );
            error::handle( e );
        }

        void reset_calibration()
        {
            rs2_error* e = nullptr;
            rs2_reset_sensor_calibration( _sensor.get(), &e );
            error::handle( e );
        }
    };

    class max_usable_range_sensor : public sensor
    {
    public:
        max_usable_range_sensor(sensor s)
            : sensor(s.get())
        {
            rs2_error* e = nullptr;
            if (rs2_is_sensor_extendable_to(_sensor.get(), RS2_EXTENSION_MAX_USABLE_RANGE_SENSOR, &e) == 0 && !e)
            {
                _sensor.reset();
            }
            error::handle(e);
        }

        operator bool() const { return _sensor.get() != nullptr; }

        float get_max_usable_depth_range() const
        {
            rs2_error* e = nullptr;
            auto res = rs2_get_max_usable_depth_range(_sensor.get(), &e);
            error::handle(e);
            return res;
        }
    };

    class debug_stream_sensor : public sensor
    {
    public:
        debug_stream_sensor( sensor s )
            : sensor( s.get() )
        {
            rs2_error * e = nullptr;
            if( rs2_is_sensor_extendable_to( _sensor.get(), RS2_EXTENSION_DEBUG_STREAM_SENSOR, &e ) == 0 && ! e )
            {
                _sensor.reset();
            }
            error::handle( e );
        }

        operator bool() const { return _sensor.get() != nullptr; }

        std::vector< stream_profile > get_debug_stream_profiles() const
        {
            std::vector< stream_profile > results;

            rs2_error * e = nullptr;
            std::shared_ptr< rs2_stream_profile_list > list(
                rs2_get_debug_stream_profiles( _sensor.get(), &e ),
                rs2_delete_stream_profiles_list );
            error::handle( e );

            auto size = rs2_get_stream_profiles_count( list.get(), &e );
            error::handle( e );

            for( auto i = 0; i < size; i++ )
            {
                stream_profile profile( rs2_get_stream_profile( list.get(), i, &e ) );
                error::handle( e );
                results.push_back( profile );
            }

            return results;
        }
    };
}
#endif // LIBREALSENSE_RS2_SENSOR_HPP