From c05edf2abcd3570f6eff36c33632fa6d527e446f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 28 Jun 2023 13:13:53 +0200 Subject: [PATCH 1/4] Implement the ObjectDistanceModule --- .../ergoCubSN000/cameraBridgeConfig.ini | 4 + .../ergoCubSN000/objectDistanceConfig.ini | 8 + ...WalkingTeleoperationFindDependencies.cmake | 3 + modules/CMakeLists.txt | 4 +- modules/ObjectDistance_module/CMakeLists.txt | 45 +++++ .../include/ObjectDistanceModule.hpp | 54 ++++++ .../src/ObjectDistanceModule.cpp | 167 ++++++++++++++++++ modules/ObjectDistance_module/src/main.cpp | 35 ++++ 8 files changed, 319 insertions(+), 1 deletion(-) create mode 100644 app/robots/ergoCubSN000/cameraBridgeConfig.ini create mode 100644 app/robots/ergoCubSN000/objectDistanceConfig.ini create mode 100644 modules/ObjectDistance_module/CMakeLists.txt create mode 100644 modules/ObjectDistance_module/include/ObjectDistanceModule.hpp create mode 100644 modules/ObjectDistance_module/src/ObjectDistanceModule.cpp create mode 100644 modules/ObjectDistance_module/src/main.cpp 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/cmake/WalkingTeleoperationFindDependencies.cmake b/cmake/WalkingTeleoperationFindDependencies.cmake index b936c86ba..61ee7b7bb 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) +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 From a7915d6ec030f622ae05f8cee2b6cf24a66479c4 Mon Sep 17 00:00:00 2001 From: iCub Date: Wed, 28 Jun 2023 18:48:46 +0200 Subject: [PATCH 2/4] Added ObjectDistance module to application --- app/scripts/ergoCub-Teleoperation-2.xml | 14 ++++++++++++++ cmake/WalkingTeleoperationFindDependencies.cmake | 2 +- 2 files changed, 15 insertions(+), 1 deletion(-) 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/cmake/WalkingTeleoperationFindDependencies.cmake b/cmake/WalkingTeleoperationFindDependencies.cmake index 61ee7b7bb..009b3ef6a 100644 --- a/cmake/WalkingTeleoperationFindDependencies.cmake +++ b/cmake/WalkingTeleoperationFindDependencies.cmake @@ -162,7 +162,7 @@ checkandset_dependency(IWear) find_package(WearableActuators QUIET) checkandset_dependency(WearableActuators) -find_package(BipedalLocomotionFramework QUIET) +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) From 86d5726a87e787d5187996121a3fe3e826790806 Mon Sep 17 00:00:00 2001 From: iCub Date: Wed, 28 Jun 2023 18:51:42 +0200 Subject: [PATCH 3/4] Added distance label visualization --- .../ergoCubSN000/openXRHeadsetParameters.ini | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) 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 From 8593af13c229e021473c36291e447f54287b6427 Mon Sep 17 00:00:00 2001 From: iCub Date: Wed, 28 Jun 2023 18:58:16 +0200 Subject: [PATCH 4/4] Using skipEyeCalibration by default. --- app/scripts/ergoCub-Teleoperation-4.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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