You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
739 lines
24 KiB
739 lines
24 KiB
// 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;
|
|
|
|
/**
|
|
* retrieve the notification category
|
|
* \return the notification category
|
|
*/
|
|
rs2_notification_category get_category() const
|
|
{
|
|
return _category;
|
|
}
|
|
/**
|
|
* retrieve the notification description
|
|
* \return the notification description
|
|
*/
|
|
std::string get_description() const
|
|
{
|
|
return _description;
|
|
}
|
|
|
|
/**
|
|
* retrieve the notification arrival timestamp
|
|
* \return the arrival timestamp
|
|
*/
|
|
double get_timestamp() const
|
|
{
|
|
return _timestamp;
|
|
}
|
|
|
|
/**
|
|
* retrieve the notification severity
|
|
* \return the severity
|
|
*/
|
|
rs2_log_severity get_severity() const
|
|
{
|
|
return _severity;
|
|
}
|
|
|
|
/**
|
|
* retrieve the notification's serialized data
|
|
* \return the serialized data (in JSON format)
|
|
*/
|
|
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;
|
|
/**
|
|
* open sensor for exclusive access, by committing to a configuration
|
|
* \param[in] profile configuration committed by the sensor
|
|
*/
|
|
void open(const stream_profile& profile) const
|
|
{
|
|
rs2_error* e = nullptr;
|
|
rs2_open(_sensor.get(),
|
|
profile.get(),
|
|
&e);
|
|
error::handle(e);
|
|
}
|
|
|
|
/**
|
|
* check if specific camera info is supported
|
|
* \param[in] info the parameter to check for support
|
|
* \return true if the parameter both exist and well-defined for the specific sensor
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* retrieve camera specific information, like versions of various internal components
|
|
* \param[in] info camera info type to retrieve
|
|
* \return the requested camera info string, in a format specific to the sensor model
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* open sensor for exclusive access, by committing to composite configuration, specifying one or more stream profiles
|
|
* this method should be used for interdependent streams, such as depth and infrared, that have to be configured together
|
|
* \param[in] profiles vector of configurations to be commited by the sensor
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* close sensor for exclusive access
|
|
* this method should be used for releasing sensor resource
|
|
*/
|
|
void close() const
|
|
{
|
|
rs2_error* e = nullptr;
|
|
rs2_close(_sensor.get(), &e);
|
|
error::handle(e);
|
|
}
|
|
|
|
/**
|
|
* Start passing frames into user provided callback
|
|
* \param[in] callback Stream callback, can be any callable object accepting rs2::frame
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* stop streaming
|
|
*/
|
|
void stop() const
|
|
{
|
|
rs2_error* e = nullptr;
|
|
rs2_stop(_sensor.get(), &e);
|
|
error::handle(e);
|
|
}
|
|
|
|
/**
|
|
* register notifications callback
|
|
* \param[in] callback notifications callback
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Retrieves the list of stream profiles supported by the sensor.
|
|
* \return list of stream profiles that given sensor can provide
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Retrieves the list of stream profiles currently streaming on the sensor.
|
|
* \return list of stream profiles that given sensor is streaming
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* get the recommended list of filters by the sensor
|
|
* \return list of filters that recommended by sensor
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/** Retrieves mapping between the units of the depth image and meters
|
|
* \return depth in meters corresponding to a depth value of 1
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Retrieve the stereoscopic baseline value from the sensor.
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Load relocalization map onto device. Only one relocalization map can be imported at a time;
|
|
* any previously existing map will be overwritten.
|
|
* The imported map exists simultaneously with the map created during the most recent tracking session after start(),
|
|
* and they are merged after the imported map is relocalized.
|
|
* This operation must be done before start().
|
|
* \param[in] lmap_buf map data as a binary blob
|
|
* \return true if success
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Get relocalization map that is currently on device, created and updated during most recent tracking session.
|
|
* Can be called before or after stop().
|
|
* \return map data as a binary blob
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Creates a named virtual landmark in the current map, known as static node.
|
|
* The static node's pose is provided relative to the origin of current coordinate system of device poses.
|
|
* This function fails if the current tracker confidence is below 3 (high confidence).
|
|
* \param[in] guid unique name of the static node (limited to 127 chars).
|
|
* If a static node with the same name already exists in the current map or the imported map, the static node is overwritten.
|
|
* \param[in] pos position of the static node in the 3D space.
|
|
* \param[in] orient_quat orientation of the static node in the 3D space, represented by a unit quaternion.
|
|
* \return true if success.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
|
|
/**
|
|
* Gets the current pose of a static node that was created in the current map or in an imported map.
|
|
* Static nodes of imported maps are available after relocalizing the imported map.
|
|
* The static node's pose is returned relative to the current origin of coordinates of device poses.
|
|
* Thus, poses of static nodes of an imported map are consistent with current device poses after relocalization.
|
|
* This function fails if the current tracker confidence is below 3 (high confidence).
|
|
* \param[in] guid unique name of the static node (limited to 127 chars).
|
|
* \param[out] pos position of the static node in the 3D space.
|
|
* \param[out] orient_quat orientation of the static node in the 3D space, represented by a unit quaternion.
|
|
* \return true if success.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* Removes a static node from the current map.
|
|
* \param[in] guid unique name of the static node (limited to 127 chars).
|
|
*/
|
|
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);
|
|
}
|
|
|
|
/** Load Wheel odometer settings from host to device
|
|
* \param[in] odometry_config_buf odometer configuration/calibration blob serialized from jsom file
|
|
* \return true on success
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/** Send wheel odometry data for each individual sensor (wheel)
|
|
* \param[in] wo_sensor_id - Zero-based index of (wheel) sensor with the same type within device
|
|
* \param[in] frame_num - Monotonocally increasing frame number, managed per sensor.
|
|
* \param[in] translational_velocity - Translational velocity in meter/sec
|
|
* \return true on success
|
|
*/
|
|
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 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; }
|
|
|
|
/** Retrieves the maximum range of the camera given the amount of ambient light in the scene.
|
|
* \return max usable range in meters
|
|
*/
|
|
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; }
|
|
|
|
/**
|
|
* Retrieves the list of debug stream profiles supported by the sensor.
|
|
* \return list of debug stream profiles that given sensor can provide
|
|
*/
|
|
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
|