18#include <librealsense2/rsutil.h>
35constexpr char rgbRes [] =
"rgbResolution";
42static std::map<std::string, RGBDSensorParamParser::RGBDParam>
params_map =
65 ss <<
"Device information: " << std::endl;
70 ss <<
" " << std::left << std::setw(20) <<
info_type <<
" : ";
75 ss <<
"N/A" << std::endl;
86 if (
sensor.is<rs2::depth_sensor>())
106 std::cout << std::endl;
110 std::cout <<
" Description : " << description << std::endl;
114 std::cout <<
" Current Value : " <<
current_value << std::endl;
118 std::cout<<std::endl;
129 unique_streams[std::make_pair(sp.stream_type(), sp.stream_index())]++;
134 std::cout <<
"Sensor consists of " <<
unique_streams.size() <<
" streams: " << std::endl;
139 std::cout <<
" - " <<
it->first.first <<
" #" <<
it->first.second << std::endl;
141 std::cout <<
"Sensor provides the following stream profiles:" << std::endl;
172 std::cout << std::endl;
197 catch (
const rs2::error&
e)
220 catch (
const rs2::error&
e)
253 catch (
const rs2::error&
e)
282 catch (
const rs2::error&
e)
333 size_t bytes_per_pixel = 0;
357 return bytes_per_pixel;
365 return YarpDistortion::YARP_PLUMB_BOB;
371 if (values.coeffs[0] == 0.0 && values.coeffs[1] == 0.0 && values.coeffs[2] == 0.0 && values.coeffs[3] == 0.0 && values.coeffs[4] == 0.0)
373 return YarpDistortion::YARP_DISTORTION_NONE;
377 return YarpDistortion::YARP_UNSUPPORTED;
402 params.toProperty(intrinsic);
417 for (
size_t j=0; j<
extrinsic.rows() - 1; j++)
419 for (
size_t i=0; i<
extrinsic.cols() - 1; i++)
433 m_paramParser(), m_verbose(
false),
435 m_needAlignment(
true), m_fps(0),
459 catch (
const rs2::error&
e)
474 catch (
const rs2::error&
e)
525 yCWarning(
REALSENSE2)<<
"Format not supported, use --verbose for more details. Setting the fallback format";
533 auto devices =
m_ctx.query_devices();
534 if (devices.size() == 0)
543 yCInfo(
REALSENSE2) <<
"Found the following " << devices.size() <<
" devices:";
544 for (
auto&& dev : devices)
629 for (
int i = 0; i < 30; i++)
635 catch (
const rs2::error&
e)
637 yCError(
REALSENSE2) <<
"m_pipeline.wait_for_frames() failed with error:"<<
"(" <<
e.what() <<
")";
661 if (
m_sensor.is<rs2::depth_sensor>())
670 if (
m_sensor.is<rs2::depth_sensor>())
767 yCWarning(
REALSENSE2) <<
"needAlignment parameter is deprecated, use alignmentFrame instead.";
804 if (!
p1.isInt32() || !
p2.isInt32() )
822 if (!
p1.isInt32() || !
p2.isInt32() )
839 std::vector<RGBDSensorParamParser::RGBDParam*> params;
843 params.push_back(&(p.second));
846 if(config.
check(
"QUANT_PARAM")) {
850 if (
quantCfg.check(
"depth_quant")) {
855 if(config.
check(
"rotateImage180")){
858 yCInfo(
REALSENSE2) <<
"parameter rotateImage180 enabled, the image is rotated";
863 if (config.
check(
"DEPTH_FILTER")) {
866 if (
filterCfg.check(
"enableHoleFilling") &&
filterCfg.find(
"enableHoleFilling").asBool()) {
869 if (
filterCfg.check(
"holeFillingMode")) {
877 if(config.
check(
"rotateIRImage180")){
880 yCInfo(
REALSENSE2) <<
"parameter rotateIRImage180 enabled, infrared images are rotated";
885 if (config.
check(
"serialnumber")) {
890 if (config.
check(
"stereoMode")) {
1020 horizontalFov = fov[0];
1021 verticalFov = fov[1];
1056 horizontalFov = fov[0];
1057 verticalFov = fov[1];
1126 catch (
const rs2::error&
e)
1128 yCError(
REALSENSE2) <<
"m_pipeline.wait_for_frames() failed with error:"<<
"(" <<
e.what() <<
")";
1135 data = align.process(data);
1148 catch (
const rs2::error&
e)
1150 yCError(
REALSENSE2) <<
"m_pipeline.wait_for_frames() failed with error:"<<
"(" <<
e.what() <<
")";
1157 data = align.process(data);
1199 if (timeStamp !=
nullptr)
1231 for(
int i = 0; i < w * h; i++)
1245 if (timeStamp !=
nullptr)
1260 catch (
const rs2::error&
e)
1262 yCError(
REALSENSE2) <<
"m_pipeline.wait_for_frames() failed with error:"<<
"(" <<
e.what() <<
")";
1269 data = align.process(data);
1376 yCError(
REALSENSE2) <<
"Something went wrong setting the feature requested, run the device with --verbose for the supported options";
1437 yCError(
REALSENSE2) <<
"Something went wrong setting the feature requested, run the device with --verbose for the supported options";
1489 yCError(
REALSENSE2) <<
"Feature does not have OnOff.. call hasOnOff() to know if a specific feature support OnOff mode";
1519 yCError(
REALSENSE2) <<
"Feature does not have OnOff.. call hasOnOff() to know if a specific feature support OnOff mode";
1522 float response = 0.0;
1665 else if (res == 1.0)
1712 catch (
const rs2::error&
e)
1714 yCError(
REALSENSE2) <<
"m_pipeline.wait_for_frames() failed with error:"<<
"(" <<
e.what() <<
")";
1719 rs2::video_frame
frm1 = data.get_infrared_frame(1);
1720 rs2::video_frame
frm2 = data.get_infrared_frame(2);
1732 imgL.setExternal((
unsigned char*) (
frm1.get_data()),
frm1.get_width(),
frm1.get_height());
1733 imgR.setExternal((
unsigned char*) (
frm2.get_data()),
frm2.get_width(),
frm2.get_height());
1740 for (
size_t y = 0; y <
imgL.height(); y++)
1742 for (
size_t x = 0; x <
imgL.width(); x++)
yarp::os::Stamp m_depth_stamp
bool setActive(int feature, bool onoff) override
bool setDepthClipPlanes(double nearPlane, double farPlane) override
Set the clipping planes of the sensor.
rs2_intrinsics m_infrared_intrin
RGBDSensor_status getSensorStatus() override
bool hasManual(int feature, bool *hasManual) override
rs2_intrinsics m_color_intrin
bool getRgbMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool getDepthImage(depthImage &depthImage, Stamp *timeStamp=nullptr) override
rs2::sensor * m_depth_sensor
bool getActive(int feature, bool *isActive) override
std::vector< cameraFeature_id_t > m_supportedFeatures
bool close() override
Close the DeviceDriver.
bool setFramerate(const int _fps)
bool hasAuto(int feature, bool *hasAuto) override
int height() const override
Return the height of each frame.
bool setDepthFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the depth camera.
bool hasFeature(int feature, bool *hasFeature) override
bool getCameraDescription(CameraDescriptor *camera) override
bool getRgbSupportedConfigurations(yarp::sig::VectorOf< yarp::dev::CameraConfig > &configurations) override
bool m_depthQuantizationEnabled
bool getMode(int feature, FeatureMode *mode) override
bool setDepthResolution(int width, int height) override
Set the resolution of the depth image from the camera.
bool setRgbFOV(double horizontalFov, double verticalFov) override
Set the field of view (FOV) of the rgb camera.
rs2::pipeline_profile m_profile
rs2_intrinsics m_depth_intrin
rs2_stream m_alignment_stream
int getDepthHeight() override
Return the height of each frame.
bool setRgbMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
rs2_extrinsics m_depth_to_color
double getDepthAccuracy() override
bool setDepthMirroring(bool mirror) override
Set the mirroring setting of the sensor.
bool hasOnOff(int feature, bool *HasOnOff) override
void updateTransformations()
bool hasOnePush(int feature, bool *hasOnePush) override
std::string getLastErrorMsg(Stamp *timeStamp=NULL) override
int getDepthWidth() override
Return the height of each frame.
bool getRgbResolution(int &width, int &height) override
Get the resolution of the rgb image from the camera.
bool getDepthFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the depth camera.
bool setFeature(int feature, double value) override
bool setOnePush(int feature) override
bool setDepthAccuracy(double accuracy) override
Set the minimum detectable variation in distance [meter] when possible.
rs2::sensor * m_color_sensor
bool getRgbFOV(double &horizontalFov, double &verticalFov) override
Get the field of view (FOV) of the rgb camera.
bool getRgbImage(FlexImage &rgbImage, Stamp *timeStamp=nullptr) override
Get the rgb frame from the device.
rs2_extrinsics m_color_to_depth
bool getImages(FlexImage &colorFrame, depthImage &depthFrame, Stamp *colorStamp=NULL, Stamp *depthStamp=NULL) override
yarp::os::Stamp m_rgb_stamp
bool getExtrinsicParam(yarp::sig::Matrix &extrinsic) override
Get the extrinsic parameters from the device.
bool getRgbIntrinsicParam(Property &intrinsic) override
int getRgbHeight() override
Return the height of each frame.
bool getDepthIntrinsicParam(Property &intrinsic) override
std::string m_serialnumber
bool getImage(yarp::sig::ImageOf< yarp::sig::PixelMono > &image) override
Get an image from the frame grabber.
int width() const override
Return the width of each frame.
bool initializeRealsenseDevice()
int getRgbWidth() override
Return the width of each frame.
yarp::dev::RGBDSensorParamParser m_paramParser
rs2::hole_filling_filter m_holeFillingFilter
bool setMode(int feature, FeatureMode mode) override
std::vector< rs2::sensor > m_sensors
bool getFeature(int feature, double *value) override
bool setRgbResolution(int width, int height) override
Set the resolution of the rgb image from the camera.
bool getDepthClipPlanes(double &nearPlane, double &farPlane) override
Get the clipping planes of the sensor.
bool getDepthMirroring(bool &mirror) override
Get the mirroring setting of the sensor.
bool parseParam(const yarp::os::Searchable &config, std::vector< RGBDParam * > ¶ms)
parseParam, parse the params stored in a Searchable.
yarp::sig::IntrinsicParams rgbIntrinsic
yarp::sig::IntrinsicParams depthIntrinsic
A mini-server for performing network communication in the background.
A class for storing options and configuration information.
A base class for nested structures that can be searched.
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
A single value (typically within a Bottle).
virtual bool isString() const
Checks if value is a string.
virtual bool isBool() const
Checks if value is a boolean.
virtual bool asBool() const
Get boolean value.
virtual std::string asString() const
Get string value.
CameraDistortionType type
Image class with user control of representation details.
yarp::sig::DistortionModelData distortionModel
static std::map< std::string, RGBDSensorParamParser::RGBDParam > params_map
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
@ YARP_FEATURE_SATURATION
@ YARP_FEATURE_FRAME_RATE
@ YARP_FEATURE_WHITE_BALANCE
An interface to the operating system, including Port based communication.
bool horzConcat(const yarp::sig::Image &inImgL, const yarp::sig::Image &inImgR, yarp::sig::Image &outImg)
Concatenate horizontally two images of the same size in one with double width.
typename vector_selector< T >::type VectorOf
constexpr char clipPlanes[]
static bool setOption(rs2_option option, const rs2::sensor *sensor, float value)
static int pixFormatToCode(const rs2_format p)
static void print_supported_options(const rs2::sensor &sensor)
static const std::map< std::string, rs2_stream > stringRSStreamMap
constexpr char needAlignment[]
static bool getOption(rs2_option option, const rs2::sensor *sensor, float &value)
static std::string get_device_information(const rs2::device &dev)
static bool optionValue2Perc(rs2_option option, const rs2::sensor *sensor, float &perc, const float &value)
static std::map< std::string, RGBDSensorParamParser::RGBDParam > params_map
static YarpDistortion rsDistToYarpDist(const rs2_distortion dist, const rs2_intrinsics &values)
static bool setExtrinsicParam(Matrix &extrinsic, const rs2_extrinsics &values)
constexpr char enableEmitter[]
static bool setIntrinsic(Property &intrinsic, const rs2_intrinsics &values)
constexpr char framerate[]
constexpr char alignmentFrame[]
static size_t bytesPerPixel(const rs2_format format)
static void settingErrorMsg(const string &error, bool &ret)
static bool optionPerc2Value(rs2_option option, const rs2::sensor *sensor, const float &perc, float &value)
constexpr char depthRes[]
static bool isSupportedFormat(const rs2::sensor &sensor, const int width, const int height, const int fps, bool verbose=false)
constexpr char accuracy[]
std::string deviceDescription
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).