Merge pull request #1878 from Nicogene/fixRGBDSensors

Various fixes rgbd sensors/wrapper

doc/release/v3_1_1.md

@@ -22,10 +22,12 @@ Bug Fixes
 * Fixed `write()` in BufferedPort after interrupting-resuming(#1834).
 * Fixed assertion when `YARP_FORWARD_LOG_ENABLE=1` (#1851).
 
-#### YARP_dev
+#### `YARP_dev`
 
 * Fixed `IControlLimits.h` not being a self-sufficient header (#1845).
 * Added missing `YARP_dev_API` to `IRobotDescription`.
+* Made optional the view of `IFrameGrabberControls` in `RGBDSensorWrapper`
+  (#1875).
 
 #### yarpidl_thrift
 
@@ -46,6 +48,11 @@ Bug Fixes
 
 * Fixed memory leak when using `cvLoad(...)`.
 
+### Devices
+
+#### `realsense2`
+
+* Fixed `set/getDepthAccuracy` methods (#1877).
 
 Contributors
 ------------

src/devices/realsense2/realsense2Driver.cpp

@@ -803,7 +803,12 @@ bool realsense2Driver::setDepthFOV(double horizontalFov, double verticalFov)
 
 bool realsense2Driver::setDepthAccuracy(double accuracy)
 {
-    return setOption(RS2_OPTION_ACCURACY, m_depth_sensor, accuracy);;
+    std::lock_guard<std::mutex> guard(m_mutex);
+    bool ok = setOption(RS2_OPTION_DEPTH_UNITS, m_depth_sensor, accuracy);
+    if (ok) {
+        m_scale = accuracy;
+    }
+    return ok;
 }
 
 bool realsense2Driver::getRgbFOV(double &horizontalFov, double &verticalFov)
@@ -903,7 +908,7 @@ bool realsense2Driver::getDepthIntrinsicParam(Property& intrinsic)
 double realsense2Driver::getDepthAccuracy()
 {
     float accuracy = 0.0;
-    if (getOption(RS2_OPTION_ACCURACY, m_depth_sensor, accuracy))
+    if (getOption(RS2_OPTION_DEPTH_UNITS, m_depth_sensor, accuracy))
     {
         return accuracy;
     }

src/devices/realsense2/realsense2Driver.h

@@ -68,6 +68,8 @@ namespace yarp
  *   Parameters used by this device are:
  * | Parameter name               | SubParameter        | Type                |  Read / write   | Units          | Default Value | Required                         | Description                                                                            | Notes                                                                 |
  * |:----------------------------:|:-------------------:|:-------------------:|:---------------:|:--------------:|:-------------:|:--------------------------------:|:--------------------------------------------------------------------------------------:|:---------------------------------------------------------------------:|
+ * |  stereoMode                  |      -              | bool                |  Read / write   |                |   false       |  No(see notes)                   | Flag for using the realsense as stereo camera                                          |  This option is to use it with yarp::dev::ServerGrabber as network wrapper. The stereo images provided are raw images(yarp::sig::PixelMono) and note that not all the realsense devices have the stereo streams. |
+ * |  verbose                     |      -              | bool                |  Read / write   |                |   false       |  No                              | Flag for enabling debug prints                                                         |                                                                       |
  * |  SETTINGS                    |      -              | group               |  Read / write   | -              |   -           |  Yes                             | Initial setting of the device.                                                         |  Properties must be read/writable in order for setting to work        |
  * |                              |  rgbResolution      | int, int            |  Read / write   | pixels         |   -           |  Yes                             | Size of rgb image in pixels                                                            |  2 values expected as height, width                                   |
  * |                              |  depthResolution    | int, int            |  Read / write   | pixels         |   -           |  Yes                             | Size of depth image in pixels                                                          |  Values are height, width                                             |
@@ -78,7 +80,7 @@ namespace yarp
  * |  HW_DESCRIPTION              |      -              |  group              |                 | -              |   -           |  Yes                             | Hardware description of device property.                                               |  Read only property. Setting will be disabled                         |
  * |                              |  clipPlanes         | double, double      |  Read / write   | meters         |   -           |  No                              | Minimum and maximum distance at which an object is seen by the depth sensor            |  parameter introduced mainly for simulated sensors, it can be used to set the clip planes if Openni gives wrong values |
  *
- * Configuration file using .ini format, using subdevice keyword.
+ * Configuration file using .ini format, for using as RGBD device:
  *
  * \code{.unparsed}
 
@@ -96,6 +98,24 @@ needAlignment   true
 [HW_DESCRIPTION]
 clipPlanes (0.2 10.0)
  *
+ * \endcode
+ *
+ * Configuration file using .ini format, for using as stereo camera:
+ * \code{.unparsed}
+device       grabberDual
+subdevice    realsense2
+name         /stereoCamera
+capabilities RAW
+stereoMode   true
+
+[SETTINGS]
+rgbResolution   (640 480)
+depthResolution   (640 480)
+framerate       30
+enableEmitter   false
+
+[HW_DESCRIPTION]
+clipPlanes (0.2 10.0)
  * \endcode
  */
 

src/libYARP_dev/include/yarp/dev/IVisualParams.h

@@ -9,6 +9,7 @@
 #ifndef YARP_DEV_IVISUALPARAMS_H
 #define YARP_DEV_IVISUALPARAMS_H
 
+#include <yarp/dev/api.h>
 #include <yarp/os/Vocab.h>
 #include <yarp/os/Property.h>
 #include <yarp/sig/Image.h>
@@ -248,14 +249,14 @@ class YARP_dev_API yarp::dev::IDepthVisualParams
     virtual bool getDepthIntrinsicParam(yarp::os::Property &intrinsic) = 0;
 
     /**
-     * Get the accuracy of the depth measure in [meter]
-     * @return the accuracy of the sensor in meters.
+     * Get the minimum detectable variation in distance [meter]
+     * @return the sensor resolution in meters.
      */
     virtual double getDepthAccuracy() = 0;
 
     /**
-     * Set the accuracy of the depth measure in [meter] when possible
-     * @param the accuracy of the sensor in meters.
+     * Set the minimum detectable variation in distance [meter] when possible
+     * @param the desired resolution in meters.
      * @return true on success
      */
     virtual bool setDepthAccuracy(double accuracy) = 0;

src/libYARP_dev/src/FrameGrabberControlImpl.cpp

@@ -243,7 +243,7 @@ bool FrameGrabberControls_Parser::respond(const yarp::os::Bottle& cmd, yarp::os:
 
     if(!fgCtrl)
     {
-        yError() << " Selected camera device has no IFrameGrabberControl2 interface";
+        yError() << " Selected camera device has no IFrameGrabberControl interface";
         return false;
     }
 

src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.cpp

@@ -385,22 +385,6 @@ bool RGBDSensorWrapper::openAndAttachSubDevice(Searchable& prop)
     if(!attach(subDeviceOwned))
         return false;
 
-    if(!rgbdParser.configure(sensor_p) || !rgbdParser.configure(fgCtrl))
-    {
-        yError() << "RGBD wrapper: error configuring interfaces for parsers";
-        return false;
-    }
-
-    /*
-    bool conf = rgbParser.configure(rgbVis_p);
-    conf &= depthParser.configure(depthVis_p);
-
-    if(!conf)
-    {
-        yError() << "RGBD wrapper: error configuring interfaces for parsers";
-        return false;
-    }
-    */
     return true;
 }
 
@@ -577,14 +561,14 @@ bool RGBDSensorWrapper::attach(yarp::dev::IRGBDSensor *s)
         return false;
     }
     sensor_p = s;
-    if(!rgbdParser.configure(sensor_p) )
+    if(!rgbdParser.configure(sensor_p))
     {
         yError() << "RGBD wrapper: error configuring interfaces for parsers";
         return false;
     }
     if (fgCtrl)
     {
-        if(!rgbdParser.configure(fgCtrl) )
+        if(!rgbdParser.configure(fgCtrl))
         {
             yError() << "RGBD wrapper: error configuring interfaces for parsers";
             return false;
@@ -609,11 +593,17 @@ bool RGBDSensorWrapper::attach(PolyDriver* poly)
         return false;
     }
 
-    if(!rgbdParser.configure(sensor_p) || !rgbdParser.configure(fgCtrl))
+    if(!rgbdParser.configure(sensor_p))
     {
-        yError() << "RGBD wrapper: error configuring interfaces for parsers";
+        yError() << "RGBD wrapper: error configuring IRGBD interface";
         return false;
     }
+
+    if (!rgbdParser.configure(fgCtrl))
+    {
+        yWarning() <<"RGBDWrapper: interface IFrameGrabberControl not implemented by the device";
+    }
+
     PeriodicThread::setPeriod(period);
     return PeriodicThread::start();
 }