Implement the ObjectDistanceModule

app/robots/ergoCubSN000/cameraBridgeConfig.ini

@@ -0,0 +1,4 @@
+stream_cameras true
+
+[Cameras]
+rgbd_cameras_list ("realsense")

app/robots/ergoCubSN000/objectDistanceConfig.ini

@@ -0,0 +1,8 @@
+name                    ObjectDistance
+
+[CAMERA_DRIVER]
+remote_image_port /depthCamera/rgbImage:o 
+remote_depth_port /depthCamera/depthImage:o
+remote_rpc_port /depthCamera/rpc:i
+
+[include CAMERA_BRIDGE cameraBridgeConfig.ini]

app/robots/ergoCubSN000/openXRHeadsetParameters.ini

@@ -18,14 +18,14 @@ mono_eye    true
 vr_poses_prediction_in_ms 0
 
 ### Camera correction
-eye_z_position      -0.8
+eye_z_position      -1.0
 
 ### Custom frames
 custom_poses       3
 
 ### Gui params
 gui_elements       0
-labels             4
+labels             5
 slides             2
 
 [CUSTOM_POSE_0]
@@ -127,6 +127,25 @@ automatically_enabled false
 disable_timeout_in_S  2.0
 port_id               risk_label
 
+[LABEL_4]
+width                 0.15
+height                0.05
+x                     -0.2
+y                     0.0
+z                     -0.5
+prefix                "Distance: "
+suffix                "m"
+pixel_size            256
+color                 (1.0, 1.0, 1.0, 1.0)
+background_color      (0.0, 0.0, 0.0, 0.5)
+font                  "Roboto/Roboto-Medium.ttf"
+horizontal_alignement center
+vertical_alignement   center
+automatically_enabled false
+disable_timeout_in_S  2.0
+port_id               distance_label
+follow_eyes           true
+
 [SLIDE_0]
 width              0.1
 height             0.1

app/scripts/ergoCub-Teleoperation-2.xml

@@ -21,12 +21,26 @@
     <parameters>--config realsense2.xml</parameters>
     <node>ergocub-head</node>
   </module>
+
+  <module>
+    <name>ObjectDistance</name>
+    <dependencies>
+      <port timeout="15.0">/depthCamera/depthImage:o</port>
+    </dependencies>
+    <node>ergocub000-lap</node>
+  </module>
 
   <!-- Connections -->
   <connection>
     <from>/depthCamera/rgbImage:o</from>
     <to>/headset/display:i</to>
     <protocol>mjpeg</protocol>
   </connection>
+
+  <connection>
+    <from>/ObjectDistanceModule/distance:o</from>
+    <to>/headset/distance_label</to>
+    <protocol>fast_tcp</protocol>
+  </connection>
 
 </application>

app/scripts/ergoCub-Teleoperation-4.xml

@@ -19,7 +19,7 @@
 
   <module>
     <name>SRanipalModule</name>
-    <parameters>--forceEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt</parameters>
+    <parameters>--skipEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt</parameters>
     <node>icub-virtualizer</node>
   </module>
 

cmake/WalkingTeleoperationFindDependencies.cmake

@@ -162,9 +162,12 @@ checkandset_dependency(IWear)
 find_package(WearableActuators QUIET)
 checkandset_dependency(WearableActuators)
 
+find_package(BipedalLocomotionFramework QUIET COMPONENTS PerceptionInterfaceYarpImplementation ParametersHandlerYarpImplementation)
+checkandset_dependency(BipedalLocomotionFramework)
 
 WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_XsensModule "Compile Xsens Module?" ON WALKING_TELEOPERATION_HAS_HumanDynamicsEstimation OFF)
 WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_VirtualizerModule "Compile Virtualizer Module?" ON WALKING_TELEOPERATION_HAS_CybSDK OFF)
 WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_FaceExpressionsRetargetingModule "Compile Face Expressions Module?" ON WALKING_TELEOPERATION_USE_libfvad OFF)
 WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_SRanipalModule "Compile SRanipal Module?" ON WALKING_TELEOPERATION_USE_SRanipalSDK OFF)
 WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_HapticGloveModule "Compile Haptic Glove Module?" ON "WALKING_TELEOPERATION_USE_IWear;WALKING_TELEOPERATION_USE_WearableActuators" OFF)
+WALKING_TELEOPERATION_dependent_option(WALKING_TELEOPERATION_COMPILE_ObjectDistanceModule "Compile Object Distance Module?" ON "WALKING_TELEOPERATION_USE_BipedalLocomotionFramework" OFF)

modules/CMakeLists.txt

@@ -25,4 +25,6 @@ if(WALKING_TELEOPERATION_COMPILE_HapticGloveModule)
     add_subdirectory(HapticGlove_module)
 endif()
 
-
+if(WALKING_TELEOPERATION_COMPILE_ObjectDistanceModule)
+    add_subdirectory(ObjectDistance_module)
+endif()

modules/ObjectDistance_module/CMakeLists.txt

@@ -0,0 +1,45 @@
+# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT)
+# All Rights Reserved.
+# Authors: Mohamed Babiker Mohamed Elobaid <[email protected]>
+#          Giulio Romualdi <[email protected]>
+#
+# set target name
+
+set(EXE_TARGET_NAME ObjectDistance)
+
+
+option(ENABLE_RPATH "Enable RPATH for this library" ON)
+mark_as_advanced(ENABLE_RPATH)
+include(AddInstallRPATHSupport)
+add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}"
+  LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}"
+  INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}"
+  DEPENDS ENABLE_RPATH
+  USE_LINK_PATH)
+
+# set cpp files
+set(${EXE_TARGET_NAME}_SRC
+  src/main.cpp
+  src/ObjectDistanceModule.cpp
+  )
+
+# set hpp files
+set(${EXE_TARGET_NAME}_HDR
+  include/ObjectDistanceModule.hpp
+  )
+
+# add an executable to the project using the specified source files.
+add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR})
+
+# add include directories to the build.
+target_include_directories(${EXE_TARGET_NAME} PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/include )
+
+target_link_libraries(${EXE_TARGET_NAME}
+  ${YARP_LIBRARIES}
+  Eigen3::Eigen
+  BipedalLocomotion::PerceptionInterfaceYarpImplementation
+  BipedalLocomotion::ParametersHandlerYarpImplementation
+  )
+
+install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin)

modules/ObjectDistance_module/include/ObjectDistanceModule.hpp

@@ -0,0 +1,54 @@
+/**
+ * @file module.h
+ * @authors
+ * @copyright
+ */
+
+#ifndef WALKING_TELEOPERATION_OBJECT_DISTANCE_MODULE_H
+#define WALKING_TELEOPERATION_OBJECT_DISTANCE_MODULE_H
+
+// std
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+// YARP
+#include <yarp/os/RFModule.h>
+
+#include <BipedalLocomotion/RobotInterface/YarpCameraBridge.h>
+
+class ObjectDistanceModule : public yarp::os::RFModule
+{
+    yarp::dev::PolyDriver m_cameraDevice;
+    BipedalLocomotion::RobotInterface::YarpCameraBridge m_cameraBridge;
+    yarp::os::BufferedPort<yarp::os::Bottle> m_outputPort;
+
+public:
+    /**
+     * Get the period of the RFModule.
+     * @return the period of the module.
+     */
+    double getPeriod() override;
+
+    /**
+     * Main function of the RFModule.
+     * @return true in case of success and false otherwise.
+     */
+    bool updateModule() override;
+
+    /**
+     * Configure the RFModule.
+     * @param rf is the reference to a resource finder object
+     * @return true in case of success and false otherwise.
+     */
+    bool configure(yarp::os::ResourceFinder& rf) override;
+
+    /**
+     * Close the RFModule.
+     * @return true in case of success and false otherwise.
+     */
+    bool close() override;
+};
+
+#endif // WALKING_TELEOPERATION_OBJECT_DISTANCE_MODULE_H

modules/ObjectDistance_module/src/ObjectDistanceModule.cpp

@@ -0,0 +1,167 @@
+/**
+ * @file module.cpp
+ * @authors
+ * @copyright
+ */
+
+#include <ObjectDistanceModule.hpp>
+
+#include <iostream>
+
+#include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+#include <yarp/cv/Cv.h>
+#include <yarp/sig/Image.h>
+
+#include <opencv2/opencv.hpp>
+
+double ObjectDistanceModule::getPeriod()
+{
+    return 0.05;
+}
+
+bool ObjectDistanceModule::configure(yarp::os::ResourceFinder& rf)
+{
+
+    this->setName("ObjectDistanceModule");
+
+    constexpr auto logPrefix = "[ObjectDistanceModule::configure]";
+
+    auto parameterHandler
+        = std::make_shared<BipedalLocomotion::ParametersHandler::YarpImplementation>();
+    parameterHandler->set(rf);
+
+    auto getParameter
+        = [logPrefix](
+              std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+              const std::string& paramName,
+              auto& param) -> bool {
+        auto ptr = handler.lock();
+        if (ptr == nullptr)
+        {
+            return false;
+            BipedalLocomotion::log()->error("{} Invalid parameter handler.", logPrefix);
+        }
+
+        if (!ptr->getParameter(paramName, param))
+        {
+            BipedalLocomotion::log()->error("{} Unable to find the parameter named: {}.", //
+                                            logPrefix,
+                                            paramName);
+            return false;
+        }
+
+        return true;
+    };
+
+    std::string remoteImagePort, remoteDepthPort, remoteRpcPort;
+    if (!getParameter(
+            parameterHandler->getGroup("CAMERA_DRIVER"), "remote_image_port", remoteImagePort)
+        || !getParameter(
+            parameterHandler->getGroup("CAMERA_DRIVER"), "remote_depth_port", remoteDepthPort)
+        || !getParameter(
+            parameterHandler->getGroup("CAMERA_DRIVER"), "remote_rpc_port", remoteRpcPort))
+    {
+        return false;
+    }
+
+    yarp::os::Property options;
+    options.put("device", "RGBDSensorClient");
+    options.put("localImagePort", "/" + this->getName() + "/depthCamera/rgbImage:i");
+    options.put("localDepthPort", "/" + this->getName() + "/depthCamera/depthImage:i");
+    options.put("localRpcPort", "/" + this->getName() + "/depthCamera/rpc:o");
+    options.put("ImageCarrier", "mjpeg");
+    options.put("DepthCarrier", "tcp+send.portmonitor+file.depthimage_compression_zlib+recv.portmonitor+file.depthimage_compression_zlib+type.dll");
+    options.put("remoteImagePort", remoteImagePort);
+    options.put("remoteDepthPort", remoteDepthPort);
+    options.put("remoteRpcPort", remoteRpcPort);
+
+    if (!m_cameraDevice.open(options))
+    {
+        BipedalLocomotion::log()->error("{} Unable to open the camera device.", logPrefix);
+        return false;
+    }
+
+    // initialize the camera bridge
+    if (!m_cameraBridge.initialize(parameterHandler->getGroup("CAMERA_BRIDGE")))
+    {
+        BipedalLocomotion::log()->error("{} Unable to configure the 'Camera bridge'.", logPrefix);
+        return false;
+    }
+
+    yarp::dev::PolyDriverList list;
+    list.push(&m_cameraDevice, "realsense");
+    if (!m_cameraBridge.setDriversList(list))
+    {
+        BipedalLocomotion::log()->error("{} Unable to set the driver list in the 'Camera bridge'.",
+                                        logPrefix);
+        return false;
+    }
+
+    yarp::os::Time::delay(3);
+
+    m_outputPort.open("/ObjectDistanceModule/distance:o");
+
+    BipedalLocomotion::log()->info("{} Configuration done", logPrefix);
+
+    return true;
+}
+
+bool ObjectDistanceModule::updateModule()
+{
+    constexpr auto logPrefix = "[ObjectDistanceModule::updateModule]";
+
+    if (m_cameraBridge.getMetaData().sensorsList.rgbdCamerasList.size() != 1)
+    {
+        BipedalLocomotion::log()->error(
+            "{} The number of RGBD camera should be equal to 1. Provided {}.",
+            logPrefix,
+            m_cameraBridge.getMetaData().sensorsList.rgbdCamerasList.size());
+        return false;
+    }
+
+    for (const std::string& cameraName : m_cameraBridge.getMetaData().sensorsList.rgbdCamerasList)
+    {
+        // get the image
+        cv::Mat frame;
+        if (!m_cameraBridge.getDepthImage(cameraName, frame))
+        {
+            BipedalLocomotion::log()->error(
+                "{} Unable to get the image for the camera named: {}.", logPrefix, cameraName);
+            return false;
+        }
+
+        yarp::sig::ImageOf<yarp::sig::PixelFloat> imgYarp
+            = yarp::cv::fromCvMat<yarp::sig::PixelFloat>(frame);
+
+        double average = 0;
+        int elements = 0;
+
+        for (int i = frame.rows / 2 - 5; i < frame.rows / 2 + 5; i++)
+        {
+            for (int j = frame.cols / 2 - 5; j < frame.cols / 2 + 5; j++)
+            {
+                elements++;
+                average += imgYarp(i, j);
+            }
+        }
+
+        average = average / elements;
+
+        std::stringstream stream;
+        stream << std::fixed << std::setprecision(2) << average;
+        std::string s = stream.str();
+
+        yarp::os::Bottle& data = m_outputPort.prepare();
+        data.clear();
+        data.addString(s);
+        m_outputPort.write();
+    }
+
+    return true;
+}
+
+bool ObjectDistanceModule::close()
+{
+    return true;
+}