diff --git a/app/robots/ergoCubSN000/cameraBridgeConfig.ini b/app/robots/ergoCubSN000/cameraBridgeConfig.ini
new file mode 100644
index 000000000..1fa16c38c
--- /dev/null
+++ b/app/robots/ergoCubSN000/cameraBridgeConfig.ini
@@ -0,0 +1,4 @@
+stream_cameras true
+
+[Cameras]
+rgbd_cameras_list ("realsense")
\ No newline at end of file
diff --git a/app/robots/ergoCubSN000/objectDistanceConfig.ini b/app/robots/ergoCubSN000/objectDistanceConfig.ini
new file mode 100644
index 000000000..2ce86b578
--- /dev/null
+++ b/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]
\ No newline at end of file
diff --git a/app/robots/ergoCubSN000/openXRHeadsetParameters.ini b/app/robots/ergoCubSN000/openXRHeadsetParameters.ini
index d80935b15..9f3b744a5 100644
--- a/app/robots/ergoCubSN000/openXRHeadsetParameters.ini
+++ b/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
diff --git a/app/scripts/ergoCub-Teleoperation-2.xml b/app/scripts/ergoCub-Teleoperation-2.xml
index 4313ad40e..59b3955fd 100644
--- a/app/scripts/ergoCub-Teleoperation-2.xml
+++ b/app/scripts/ergoCub-Teleoperation-2.xml
@@ -21,6 +21,14 @@
--config realsense2.xml
ergocub-head
+
+
+ ObjectDistance
+
+ /depthCamera/depthImage:o
+
+ ergocub000-lap
+
@@ -28,5 +36,11 @@
/headset/display:i
mjpeg
+
+
+ /ObjectDistanceModule/distance:o
+ /headset/distance_label
+ fast_tcp
+
diff --git a/app/scripts/ergoCub-Teleoperation-4.xml b/app/scripts/ergoCub-Teleoperation-4.xml
index c3e1cb887..18acb395a 100644
--- a/app/scripts/ergoCub-Teleoperation-4.xml
+++ b/app/scripts/ergoCub-Teleoperation-4.xml
@@ -19,7 +19,7 @@
SRanipalModule
- --forceEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt
+ --skipEyeCalibration --robot ergocub --noEyebrows --noEyelids --gazeVelocityGain 2.0 --gazeDeadzone 0.1 --gazeDeadzoneActivationOffset 0.1 --gazeDeadzoneMinActivationTime 0.2 --eyesVersionName none --eyesVergenceName none --eyesTiltName camera_tilt
icub-virtualizer
diff --git a/cmake/WalkingTeleoperationFindDependencies.cmake b/cmake/WalkingTeleoperationFindDependencies.cmake
index b936c86ba..009b3ef6a 100644
--- a/cmake/WalkingTeleoperationFindDependencies.cmake
+++ b/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)
diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt
index e8239e07c..fc8ab8471 100644
--- a/modules/CMakeLists.txt
+++ b/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()
diff --git a/modules/ObjectDistance_module/CMakeLists.txt b/modules/ObjectDistance_module/CMakeLists.txt
new file mode 100644
index 000000000..770fdf308
--- /dev/null
+++ b/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
+# Giulio Romualdi
+#
+# 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)
diff --git a/modules/ObjectDistance_module/include/ObjectDistanceModule.hpp b/modules/ObjectDistance_module/include/ObjectDistanceModule.hpp
new file mode 100644
index 000000000..f6b522fa3
--- /dev/null
+++ b/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
+#include
+#include
+#include
+
+// YARP
+#include
+
+#include
+
+class ObjectDistanceModule : public yarp::os::RFModule
+{
+ yarp::dev::PolyDriver m_cameraDevice;
+ BipedalLocomotion::RobotInterface::YarpCameraBridge m_cameraBridge;
+ yarp::os::BufferedPort 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
\ No newline at end of file
diff --git a/modules/ObjectDistance_module/src/ObjectDistanceModule.cpp b/modules/ObjectDistance_module/src/ObjectDistanceModule.cpp
new file mode 100644
index 000000000..8b03da8ae
--- /dev/null
+++ b/modules/ObjectDistance_module/src/ObjectDistanceModule.cpp
@@ -0,0 +1,167 @@
+/**
+ * @file module.cpp
+ * @authors
+ * @copyright
+ */
+
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+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();
+ parameterHandler->set(rf);
+
+ auto getParameter
+ = [logPrefix](
+ std::weak_ptr 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 imgYarp
+ = yarp::cv::fromCvMat(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;
+}
\ No newline at end of file
diff --git a/modules/ObjectDistance_module/src/main.cpp b/modules/ObjectDistance_module/src/main.cpp
new file mode 100644
index 000000000..e25ec8fff
--- /dev/null
+++ b/modules/ObjectDistance_module/src/main.cpp
@@ -0,0 +1,35 @@
+/**
+ * @file main.cpp
+ * @authors
+ * @copyright
+*/
+
+// YARP
+#include
+#include
+#include
+
+#include
+
+int main(int argc, char* argv[])
+{
+ // initialize yarp network
+ yarp::os::Network yarp;
+ if (!yarp.checkNetwork())
+ {
+ yError() << "[main] Unable to find YARP network";
+ return EXIT_FAILURE;
+ }
+
+ // prepare and configure the resource finder
+ yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton();
+
+ rf.setDefaultConfigFile("objectDistanceConfig.ini");
+
+ rf.configure(argc, argv);
+
+ // create the module
+ ObjectDistanceModule module;
+
+ return module.runModule(rf);
+}
\ No newline at end of file