openrave-yarp-plugins
Loading...
Searching...
No Matches
YarpOpenraveRGBDSensor.hpp
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3#ifndef __YARP_OPENRAVE_RGBDSENSOR_HPP__
4#define __YARP_OPENRAVE_RGBDSENSOR_HPP__
5
6#include <yarp/conf/version.h>
7
8#include <yarp/dev/DeviceDriver.h>
9#include <yarp/dev/IRGBDSensor.h>
10
11#include <openrave/openrave.h>
12
13#include <boost/smart_ptr/shared_ptr.hpp>
14
15#include "YarpOpenraveBase.hpp"
16
17namespace roboticslab
18{
19
32class YarpOpenraveRGBDSensor : private YarpOpenraveBase,
33 public yarp::dev::DeviceDriver,
34 public yarp::dev::IRGBDSensor
35{
36public:
37#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
38 constexpr static auto ret_ok = yarp::dev::ReturnValue::return_code::return_value_ok;
39 constexpr static auto ret_not_ok = yarp::dev::ReturnValue::return_code::return_value_error_not_implemented_by_device;
40#endif
41
42 // ------- DeviceDriver declarations. Implementation in DeviceDriverImageImpl.cpp -------
43 bool open(yarp::os::Searchable & config) override;
44 bool close() override;
45
46 // ------- IRGBDSensor declarations. Implementation in IRGBDSensorImpl.cpp -------
47 // IRgbVisualParams interface. Look at IVisualParams.h for documentation
48#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
49 yarp::dev::ReturnValue getRgbSupportedConfigurations(std::vector<yarp::dev::CameraConfig> & configurations) override { return ret_not_ok; }
50 yarp::dev::ReturnValue getRgbResolution(int & width, int & height) override { return ret_not_ok; }
51 yarp::dev::ReturnValue setRgbResolution(int width, int height) override { return ret_not_ok; }
52 yarp::dev::ReturnValue getRgbFOV(double & horizontalFov, double & verticalFov) override { return ret_not_ok; }
53 yarp::dev::ReturnValue setRgbFOV(double horizontalFov, double verticalFov) override { return ret_not_ok; }
54#if YARP_VERSION_COMPARE(>=, 4, 1, 0) || defined(YARP_NEXT)
55 yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::sig::IntrinsicParams & intrinsic) override { return ret_not_ok; }
56#else
57 yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::os::Property & intrinsic) override { return ret_not_ok; }
58#endif
59 yarp::dev::ReturnValue getRgbMirroring(bool & mirror) override { return ret_not_ok; }
60 yarp::dev::ReturnValue setRgbMirroring(bool mirror) override { return ret_not_ok; }
61#else
62 bool getRgbSupportedConfigurations(yarp::sig::VectorOf<yarp::dev::CameraConfig> & configurations) override { return false; }
63 bool getRgbResolution(int & width, int & height) override { return false; }
64 bool setRgbResolution(int width, int height) override { return false; }
65 bool getRgbFOV(double & horizontalFov, double & verticalFov) override { return false; }
66 bool setRgbFOV(double horizontalFov, double verticalFov) override { return false; }
67 bool getRgbIntrinsicParam(yarp::os::Property & intrinsic) override { return false; }
68 bool getRgbMirroring(bool & mirror) override { return false; }
69 bool setRgbMirroring(bool mirror) override { return false; }
70#endif
71 int getRgbHeight() override { return 0; };
72 int getRgbWidth() override { return 0; };
73
74 // IDepthVisualParams interface. Look at IVisualParams.h for documentation
75#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
76 yarp::dev::ReturnValue getDepthResolution(int & width, int & height) override { return ret_not_ok; }
77 yarp::dev::ReturnValue setDepthResolution(int width, int height) override { return ret_not_ok; }
78 yarp::dev::ReturnValue getDepthFOV(double & horizontalFov, double & verticalFov) override { return ret_not_ok; }
79 yarp::dev::ReturnValue setDepthFOV(double horizontalFov, double verticalFov) override { return ret_not_ok; }
80 yarp::dev::ReturnValue getDepthAccuracy(double & accuracy) override { accuracy = 0.0; return ret_ok; }
81 yarp::dev::ReturnValue setDepthAccuracy(double accuracy) override { return ret_not_ok; }
82 yarp::dev::ReturnValue getDepthClipPlanes(double & nearPlane, double & farPlane) override { return ret_not_ok; }
83 yarp::dev::ReturnValue setDepthClipPlanes(double nearPlane, double farPlane) override { return ret_not_ok; }
84#if YARP_VERSION_COMPARE(>=, 4, 1, 0) || defined(YARP_NEXT)
85 yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::sig::IntrinsicParams & intrinsic) override { return ret_not_ok; }
86#else
87 yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::os::Property & intrinsic) override { return ret_not_ok; }
88#endif
89 yarp::dev::ReturnValue getDepthMirroring(bool & mirror) override { return ret_not_ok; }
90 yarp::dev::ReturnValue setDepthMirroring(bool mirror) override { return ret_not_ok; }
91#else
92 bool setDepthResolution(int width, int height) override { return false; }
93 bool getDepthFOV(double & horizontalFov, double & verticalFov) override { return false; }
94 bool setDepthFOV(double horizontalFov, double verticalFov) override { return false; }
95 double getDepthAccuracy() override { return 0.0; }
96 bool setDepthAccuracy(double accuracy) override { return false; }
97 bool getDepthClipPlanes(double & nearPlane, double & farPlane) override { return false; }
98 bool setDepthClipPlanes(double nearPlane, double farPlane) override { return false; }
99 bool getDepthIntrinsicParam(yarp::os::Property & intrinsic) override { return false; }
100 bool getDepthMirroring(bool & mirror) override { return false; }
101 bool setDepthMirroring(bool mirror) override { return false; }
102#endif
103 int getDepthHeight() override { return depthHeight; };
104 int getDepthWidth() override { return depthWidth; };
105
106 // IRGBDSensor specific interface methods
107#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
108 yarp::dev::ReturnValue getExtrinsicParam(yarp::sig::Matrix & extrinsic) override { return ret_not_ok; }
109 yarp::dev::ReturnValue getLastErrorMsg(std::string & message, yarp::os::Stamp * timeStamp = nullptr) override { message = ""; return ret_ok; }
110 yarp::dev::ReturnValue getRgbImage(yarp::sig::FlexImage & rgbImage, yarp::os::Stamp * timeStamp = nullptr) override;
111 yarp::dev::ReturnValue getDepthImage(yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthImage, yarp::os::Stamp * timeStamp = nullptr) override;
112 yarp::dev::ReturnValue getImages(yarp::sig::FlexImage & colorFrame, yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame, yarp::os::Stamp * colorStamp = nullptr, yarp::os::Stamp * depthStamp = nullptr) override;
113 yarp::dev::ReturnValue getSensorStatus(yarp::dev::IRGBDSensor::RGBDSensor_status & status) override;
114#else
115 bool getExtrinsicParam(yarp::sig::Matrix & extrinsic) override { return false; }
116 std::string getLastErrorMsg(yarp::os::Stamp * timeStamp = nullptr) override { return ""; }
117 bool getRgbImage(yarp::sig::FlexImage & rgbImage, yarp::os::Stamp * timeStamp = nullptr) override;
118 bool getDepthImage(yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthImage, yarp::os::Stamp * timeStamp = nullptr) override;
119 bool getImages(yarp::sig::FlexImage & colorFrame, yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame, yarp::os::Stamp * colorStamp = nullptr, yarp::os::Stamp * depthStamp = nullptr) override;
120 yarp::dev::IRGBDSensor::RGBDSensor_status getSensorStatus() override;
121#endif
122
123private:
124 int rgbHeight, rgbWidth, depthHeight, depthWidth;
125
126 bool rgb {true};
127 bool rgbReady {false};
128 bool depthReady {false};
129
130 // OpenRAVE
131 OpenRAVE::SensorBasePtr depthSensorBasePtr;
132 OpenRAVE::SensorBasePtr rgbSensorBasePtr;
133
134 boost::shared_ptr<OpenRAVE::SensorBase::LaserSensorData> depthSensorDataPtr;
135 boost::shared_ptr<OpenRAVE::SensorBase::CameraSensorData> rgbSensorDataPtr;
136};
137
138} // namespace roboticslab
139
140#endif // __YARP_OPENRAVE_RGBDSENSOR_HPP__
roboticslab::YarpOpenraveBase
Implements shared configuration functions.
Definition YarpOpenraveBase.hpp:32
roboticslab::YarpOpenraveRGBDSensor
Implements the YARP_dev IRGBDSensor, etc. interface class member functions.
Definition YarpOpenraveRGBDSensor.hpp:35
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition groups.dox:5
Generated by doxygen 1.9.8