diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index f8c03f0f92..476afce440 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.github/workflows/conda-forge-ci.yml @@ -67,13 +67,20 @@ jobs: # Compilation related dependencies mamba install vs2019_win-64 - - name: Remove icub-models [Windows] + - name: Remove icub-models and pcl [Windows] if: contains(matrix.os, 'windows') shell: bash -l {0} run: | # Due to this https://github.com/conda-forge/icub-models-feedstock/issues/18 - mamba remove icub-models + # pcl is removed as a workaround for https://github.com/ami-iit/bipedal-locomotion-framework/pull/695#issuecomment-1632208836 + # pcl can be re-added once we have a ros humble build compatible with PCL 1.13.0 + mamba remove icub-models pcl + - name: Print used environment + shell: bash -l {0} + run: | + mamba list + env - name: Configure [Linux&macOS] if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu') diff --git a/CHANGELOG.md b/CHANGELOG.md index d7e0641dfb..6d005e52c2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,11 +10,14 @@ All notable changes to this project are documented in this file. - Expose `ContactBase` methods for `DiscreteGeometryContact` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/712) - Expose the CoM trajectory and angular momentum trajectory in `MANNTrajectoryGenerator` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/712) - Expose the CoM trajectory computed by the `CentroidalMPC` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/712) +- Implement python bindings for `CameraBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) +- Implement `constructRDGBSensorClient()` in `RobotInterface` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) ### Changed - Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702) - Give the possibility to set an external wrench in the `CentroidalDynamics` instead of a pure force (https://github.com/ami-iit/bipedal-locomotion-framework/pull/705) - Use c version of `qhull` in the Planner component. This fixes the compatibility with PCL in ubuntu 20.04 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/713) +- Make `ICameraBridge::isValid()` virtual function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695) ### Fixed - Remove duplicated `find_package` in `BipedalLocomotionFrameworkDependencies.cmake` file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/709) diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 1cd1ebe13a..e3e42af244 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -29,6 +29,7 @@ get_property(pybind_headers GLOBAL PROPERTY pybind_headers) get_property(pybind_sources GLOBAL PROPERTY pybind_sources) get_property(pybind_include_dirs GLOBAL PROPERTY pybind_include_dirs) get_property(pybind_link_libraries GLOBAL PROPERTY pybind_link_libraries) +get_property(pybind_compile_definitions GLOBAL PROPERTY pybind_compile_definitions) pybind11_add_module(pybind11_blf MODULE ${CMAKE_CURRENT_BINARY_DIR}/bipedal_locomotion_framework.cpp @@ -38,6 +39,7 @@ pybind11_add_module(pybind11_blf MODULE target_compile_features(pybind11_blf PUBLIC cxx_std_17) target_include_directories(pybind11_blf PUBLIC "$") +target_compile_definitions(pybind11_blf PRIVATE ${pybind_compile_definitions}) target_link_libraries(pybind11_blf PRIVATE ${pybind_link_libraries}) diff --git a/bindings/python/RobotInterface/CMakeLists.txt b/bindings/python/RobotInterface/CMakeLists.txt index 22731f13db..aa33fc9e27 100644 --- a/bindings/python/RobotInterface/CMakeLists.txt +++ b/bindings/python/RobotInterface/CMakeLists.txt @@ -14,3 +14,38 @@ if(TARGET BipedalLocomotion::RobotInterface AND TARGET BipedalLocomotion::RobotI ) endif() + +if(TARGET BipedalLocomotion::RobotInterface + AND TARGET BipedalLocomotion::PerceptionInterface + AND TARGET BipedalLocomotion::RobotInterfaceYarpImplementation + AND TARGET BipedalLocomotion::PerceptionInterfaceYarpImplementation) + + set(cvnp_target_link ) + + # This compiles only if pybind11 is at least v2.7.0 + # Indeed we need a feature in pybind that has been introduced by this commit + # https://github.com/pybind/pybind11/commit/74a767d42921001fc4569ecee3b8726383c42ad4 + # https://github.com/pybind/pybind11/pull/2864 + if (${pybind11_VERSION} VERSION_GREATER_EQUAL "2.7.0") + FetchContent_Declare( + cvnp + GIT_REPOSITORY https://github.com/pthom/cvnp + GIT_TAG 6aeac770ccff122abf82573da1ae03e18e7a4707 + ) + FetchContent_MakeAvailable(cvnp) + set(cvnp_target_link cvnp) + endif() + + set(H_PREFIX include/BipedalLocomotion/bindings/RobotInterface) + + add_bipedal_locomotion_python_module( + NAME PerceptionInterfaceBindings + SOURCES src/PerceptionModule.cpp src/CameraBridge.cpp + HEADERS ${H_PREFIX}/PerceptionModule.h ${H_PREFIX}/CameraBridge.h + LINK_LIBRARIES BipedalLocomotion::PerceptionInterface BipedalLocomotion::PerceptionInterfaceYarpImplementation ${cvnp_target_link} + ADDITIONAL_COMPILE_DEFINITIONS pybind11_VERSION_MAJOR=${pybind11_VERSION_MAJOR} + pybind11_VERSION_MINOR=${pybind11_VERSION_MINOR} + pybind11_VERSION_PATCH=${pybind11_VERSION_PATCH} + ) + +endif() diff --git a/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/CameraBridge.h b/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/CameraBridge.h new file mode 100644 index 0000000000..711a5355d3 --- /dev/null +++ b/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/CameraBridge.h @@ -0,0 +1,27 @@ +/** + * @file CameraBridge.h + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_BRIDGE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_BRIDGE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotInterface +{ + +void CreateICameraBridge(pybind11::module& module); +void CreateYarpCameraBridge(pybind11::module& module); + +} // namespace RobotInterface +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_BRIDGE_H diff --git a/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/PerceptionModule.h b/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/PerceptionModule.h new file mode 100644 index 0000000000..2a04fbae23 --- /dev/null +++ b/bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/PerceptionModule.h @@ -0,0 +1,26 @@ +/** + * @file PerceptionModule.h + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_PERCEPTION_MODULE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_PERCEPTION_MODULE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotInterface +{ + +void CreatePerceptionModule(pybind11::module& module); + +} // namespace RobotInterface +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_PERCEPTION_MODULE_H diff --git a/bindings/python/RobotInterface/src/CameraBridge.cpp b/bindings/python/RobotInterface/src/CameraBridge.cpp new file mode 100644 index 0000000000..b18a1abdce --- /dev/null +++ b/bindings/python/RobotInterface/src/CameraBridge.cpp @@ -0,0 +1,122 @@ +/** + * @file CameraBridge.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#define PYBIND_VERSION_AT_LEAST(x, y, z) \ + ((pybind11_VERSION_MAJOR > x) \ + || ((pybind11_VERSION_MAJOR >= x) && (pybind11_VERSION_MINOR > y)) \ + || ((pybind11_VERSION_MAJOR >= x) && (pybind11_VERSION_MINOR >= y) \ + && (pybind11_VERSION_PATCH > z))) + +// This compiles only if pybind11 is at least v2.7.0 +// Indeed we need a feature in pybind that has been introduced by this commit +// https://github.com/pybind/pybind11/commit/74a767d42921001fc4569ecee3b8726383c42ad4 +// https://github.com/pybind/pybind11/pull/2864 +#if PYBIND_VERSION_AT_LEAST(2, 7, 0) +// NumPy/OpenCV compatibility +#include +#endif // PYBIND_VERSION_AT_LEAST(2, 7, 0) + +#include +#include +#include + +#include +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotInterface +{ + +void CreateICameraBridge(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::RobotInterface; + using namespace BipedalLocomotion::ParametersHandler; + + py::class_(module, "CameraBridgeOptions") + .def(py::init()) + .def_readwrite("is_rgb_camera_enabled", &CameraBridgeOptions::isRGBCameraEnabled) + .def_readwrite("is_rgbd_camera_enabled", &CameraBridgeOptions::isRGBDCameraEnabled) + .def_readwrite("rgb_img_dimensions", &CameraBridgeOptions::rgbImgDimensions) + .def_readwrite("rgbd_img_dimension", &CameraBridgeOptions::rgbdImgDimensions); + + py::class_(module, "CameraLists") + .def(py::init()) + .def_readwrite("rgb_cameras_list", &CameraLists::rgbCamerasList) + .def_readwrite("rgbd_cameras_list", &CameraLists::rgbdCamerasList); + + py::class_(module, "CameraBridgeMetaData") + .def(py::init()) + .def_readwrite("sensors_list", &CameraBridgeMetaData::sensorsList) + .def_readwrite("bridge_options", &CameraBridgeMetaData::bridgeOptions); + + py::class_ iCameraBridge(module, "ICameraBridge"); + iCameraBridge + .def( + "initialize", + [](ICameraBridge& impl, std::shared_ptr handler) -> bool { + return impl.initialize(handler); + }, + py::arg("handler")) + .def("get_meta_data", &ICameraBridge::getMetaData) + .def("is_valid", &ICameraBridge::isValid); + + // Here we require a feature in pybind that has been introduced by this commit + // https://github.com/pybind/pybind11/commit/74a767d42921001fc4569ecee3b8726383c42ad4 + // https://github.com/pybind/pybind11/pull/2864 +#if PYBIND_VERSION_AT_LEAST(2, 7, 0) + iCameraBridge + .def( + "get_color_image", + [](ICameraBridge& impl, const std::string& camName) { + std::pair ret; + ret.first = impl.getColorImage(camName, ret.second); + return ret; + }, + py::arg("cam_name")) + .def( + "get_depth_image", + [](ICameraBridge& impl, const std::string& camName) { + std::pair ret; + ret.first = impl.getDepthImage(camName, ret.second); + return ret; + }, + py::arg("cam_name")); +#endif // PYBIND_VERSION_AT_LEAST(2, 7, 0) +} + +void CreateYarpCameraBridge(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::RobotInterface; + using namespace BipedalLocomotion::System; + py::class_(module, "YarpCameraBridge") + .def(py::init()) + .def( + "set_drivers_list", + [](YarpCameraBridge& impl, std::vector polydrivers) -> bool { + yarp::dev::PolyDriverList list; + for (const auto& polydriver : polydrivers) + { + list.push(polydriver.poly.get(), polydriver.key.c_str()); + } + return impl.setDriversList(list); + }, + py::arg("polydrivers")); +} + +} // namespace RobotInterface +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/RobotInterface/src/PerceptionModule.cpp b/bindings/python/RobotInterface/src/PerceptionModule.cpp new file mode 100644 index 0000000000..65a74db15f --- /dev/null +++ b/bindings/python/RobotInterface/src/PerceptionModule.cpp @@ -0,0 +1,27 @@ +/** + * @file PerceptionModule.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotInterface +{ +void CreatePerceptionModule(pybind11::module& module) +{ + CreateICameraBridge(module); + CreateYarpCameraBridge(module); +} +} // namespace RobotInterface +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/RobotInterface/src/Polydriver.cpp b/bindings/python/RobotInterface/src/Polydriver.cpp index c7c16c1241..c4f16a7e00 100644 --- a/bindings/python/RobotInterface/src/Polydriver.cpp +++ b/bindings/python/RobotInterface/src/Polydriver.cpp @@ -43,31 +43,38 @@ void CreatePolyDriverDescriptor(pybind11::module& module) module.def( "construct_remote_control_board_remapper", - [](std::shared_ptr handler) -> PolyDriverDescriptor { + [](std::shared_ptr handler) -> PolyDriverDescriptor { return constructRemoteControlBoardRemapper(handler); }, py::arg("handler")); module.def( "construct_generic_sensor_client", - [](std::shared_ptr handler) -> PolyDriverDescriptor { + [](std::shared_ptr handler) -> PolyDriverDescriptor { return constructGenericSensorClient(handler); }, py::arg("handler")); module.def( "construct_multiple_analog_sensors_client", - [](std::shared_ptr handler) -> PolyDriverDescriptor { + [](std::shared_ptr handler) -> PolyDriverDescriptor { return constructMultipleAnalogSensorsClient(handler); }, py::arg("handler")); module.def( "construct_multiple_analog_sensors_remapper", - [](std::shared_ptr handler) -> PolyDriverDescriptor { + [](std::shared_ptr handler) -> PolyDriverDescriptor { return constructMultipleAnalogSensorsRemapper(handler); }, py::arg("handler")); + + module.def( + "construct_RGBD_sensor_client", + [](std::shared_ptr handler) -> PolyDriverDescriptor { + return constructRDGBSensorClient(handler); + }, + py::arg("handler")); } } // namespace RobotInterface diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index 9f77831127..91aeaa4cde 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -58,6 +58,10 @@ #include @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation +@cmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation +#include +@endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation + @cmakeiftarget BipedalLocomotion::FloatingBaseEstimators #include @endcmakeiftarget BipedalLocomotion::FloatingBaseEstimators @@ -161,6 +165,10 @@ PYBIND11_MODULE(bindings, m) bindings::RobotInterface::CreateModule(robotInterfaceModule); @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation + @cmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation + bindings::RobotInterface::CreatePerceptionModule(robotInterfaceModule); + @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation + @cmakeiftarget BipedalLocomotion::FloatingBaseEstimators py::module floatingBaseEstimatorModule = m.def_submodule("floating_base_estimators"); bindings::FloatingBaseEstimators::CreateModule(floatingBaseEstimatorModule); diff --git a/cmake/AddBipedalLocomotionPythonModule.cmake b/cmake/AddBipedalLocomotionPythonModule.cmake index a9d6992f3e..b552a02f4c 100644 --- a/cmake/AddBipedalLocomotionPythonModule.cmake +++ b/cmake/AddBipedalLocomotionPythonModule.cmake @@ -11,7 +11,8 @@ function(add_bipedal_locomotion_python_module) HEADERS LINK_LIBRARIES TESTS - TESTS_RUNTIME_CONDITIONS) + TESTS_RUNTIME_CONDITIONS + ADDITIONAL_COMPILE_DEFINITIONS) set(prefix "bipedal_component") @@ -29,6 +30,7 @@ function(add_bipedal_locomotion_python_module) set(subdirectories ${${prefix}_SUBDIRECTORIES}) set(tests ${${prefix}_TESTS}) set(tests_runtime_conditions ${${prefix}_TESTS_RUNTIME_CONDITIONS}) + set(additional_compile_definitions ${${prefix}_ADDITIONAL_COMPILE_DEFINITIONS}) foreach(file ${headers}) set_property(GLOBAL APPEND PROPERTY pybind_headers ${CMAKE_CURRENT_SOURCE_DIR}/${file}) @@ -57,6 +59,10 @@ function(add_bipedal_locomotion_python_module) endforeach() endif() + foreach(definition ${additional_compile_definitions}) + set_property(GLOBAL APPEND PROPERTY pybind_compile_definitions ${definition}) + endforeach() + message(STATUS "Added files for bindings named ${name} in ${PROJECT_NAME}.") endfunction() diff --git a/src/Perception/include/BipedalLocomotion/Perception/Capture/RealSense.h b/src/Perception/include/BipedalLocomotion/Perception/Capture/RealSense.h index 3b3f72ca94..338529996e 100644 --- a/src/Perception/include/BipedalLocomotion/Perception/Capture/RealSense.h +++ b/src/Perception/include/BipedalLocomotion/Perception/Capture/RealSense.h @@ -45,7 +45,11 @@ class RealSense : public BipedalLocomotion::RobotInterface::ICameraBridge, bool initialize( std::weak_ptr handler) final; - bool isValid(); + /** + * @brief Determines the validity of the object retrieved with getMetadata() + * @return True if the object is valid, false otherwise. + */ + bool isValid() const final; bool getRGBCamerasList(std::vector& rgbCamerasList) final; diff --git a/src/Perception/src/RealSense.cpp b/src/Perception/src/RealSense.cpp index ff613790b4..f778f0bda1 100644 --- a/src/Perception/src/RealSense.cpp +++ b/src/Perception/src/RealSense.cpp @@ -177,7 +177,7 @@ bool RealSense::initialize( return true; } -bool RealSense::isValid() +bool RealSense::isValid() const { if (!m_pimpl->isStreaming) { diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpCameraBridge.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpCameraBridge.h index 6de7142718..fddbcfcba5 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpCameraBridge.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpCameraBridge.h @@ -74,10 +74,10 @@ class YarpCameraBridge : public BipedalLocomotion::RobotInterface::ICameraBridge bool setDriversList(const yarp::dev::PolyDriverList& deviceDriversList); /** - * @brief Determines the validity of the object retrieved with get() + * @brief Determines the validity of the object retrieved with getMetadata() * @return True if the object is valid, false otherwise. */ - bool isValid() const; + bool isValid() const final; /** * @brief Get the object. diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpHelper.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpHelper.h index 454b06ae23..c6c03e71b1 100644 --- a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpHelper.h +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpHelper.h @@ -158,6 +158,28 @@ PolyDriverDescriptor constructMultipleAnalogSensorsRemapper( std::weak_ptr handler, const yarp::dev::PolyDriverList& polydriverList); +/** + * Helper function that can be used to build a `RGBDSensorClient` device. + * @param handler pointer to a parameter handler interface. + * @note The following parameters are taken into consideration + * | Parameter Name | Type | Description | Mandatory | + * |:----------------------------:|:--------:|:----------------------------------------------------------------------------------------------------:|:---------:| + * | `name` | `string` | Name of the camera client | Yes | + * | `local_prefix` | `string` | Prefix of the local port (e.g. the application name) | Yes | + * | `local_image_port_postfix` | `string` | Postfix of the local image port. The local port name is `/` | Yes | + * | `local_depth_port_postfix` | `string` | Postfix of the local depth port. The local port name is `/` | Yes | + * | `local_rpc_port_postfix` | `string` | Postfix of the local rpc port. The local port name is `/` | Yes | + * | `remote_image_port` | `string` | Name of the port associate to the RGB image. | Yes | + * | `remote_depth_port` | `string` | Name of the port associate to the depth image. | Yes | + * | `remote_rpc_port` | `string` | Name of the port associate to the rpc port. | Yes | + * | `image_carrier` | `string` | Carrier for the RGB image. | Yes | + * | `depth_carrier` | `string` | Carrier for the depth image. | Yes | + * @note The `RGBDSensorClient` device is implement in [yarp](https://yarp.it/git-master/classRGBDSensorClient.html). + * @return A PolyDriverDescriptor. In case of error an invalid `PolyDriverDescriptor` is returned. + */ +PolyDriverDescriptor constructRDGBSensorClient( + std::weak_ptr handler); + } // namespace RobotInterface } // namespace BipedalLocomotion diff --git a/src/RobotInterface/YarpImplementation/src/YarpHelper.cpp b/src/RobotInterface/YarpImplementation/src/YarpHelper.cpp index a560e8c809..8f4c3b0fdd 100644 --- a/src/RobotInterface/YarpImplementation/src/YarpHelper.cpp +++ b/src/RobotInterface/YarpImplementation/src/YarpHelper.cpp @@ -227,7 +227,7 @@ PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructMultipleAnalogS } std::string description; - if(!ptr->getParameter("description", description)) + if (!ptr->getParameter("description", description)) { log()->error("{} Unable to find the parameter 'description'.", errorPrefix); return PolyDriverDescriptor(); @@ -312,3 +312,76 @@ PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructMultipleAnalogS return device; } + +PolyDriverDescriptor BipedalLocomotion::RobotInterface::constructRDGBSensorClient( + std::weak_ptr handler) +{ + constexpr auto errorPrefix = "[RobotInterface::constructRDGBSensorClient]"; + + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", errorPrefix); + return PolyDriverDescriptor(); + } + + bool ok = true; + + std::string name; + ok = ok && ptr->getParameter("name", name); + + std::string localPrefix; + ok = ok && ptr->getParameter("local_prefix", localPrefix); + + std::string localImagePortNamePostfix; + ok = ok && ptr->getParameter("local_image_port_postfix", localImagePortNamePostfix); + + std::string localDepthPortNamePostfix; + ok = ok && ptr->getParameter("local_depth_port_postfix", localDepthPortNamePostfix); + + std::string localRpcPortNamePostfix; + ok = ok && ptr->getParameter("local_rpc_port_postfix", localRpcPortNamePostfix); + + std::string remoteImagePortName; + ok = ok && ptr->getParameter("remote_image_port", remoteImagePortName); + + std::string remoteDepthPortName; + ok = ok && ptr->getParameter("remote_depth_port", remoteDepthPortName); + + std::string remoteRpcPortName; + ok = ok && ptr->getParameter("remote_rpc_port", remoteRpcPortName); + + std::string imageCarrier; + ok = ok && ptr->getParameter("image_carrier", imageCarrier); + + std::string depthCarrier; + ok = ok && ptr->getParameter("depth_carrier", depthCarrier); + + if (!ok) + { + log()->error("{} Unable to get all the parameters from configuration file.", errorPrefix); + return PolyDriverDescriptor(); + } + + yarp::os::Property options; + options.put("device", "RGBDSensorClient"); + options.put("localImagePort", "/" + localPrefix + localImagePortNamePostfix); + options.put("localDepthPort", "/" + localPrefix + localDepthPortNamePostfix); + options.put("localRpcPort", "/" + localPrefix + localRpcPortNamePostfix); + options.put("ImageCarrier", imageCarrier); + options.put("DepthCarrier", depthCarrier); + options.put("remoteImagePort", remoteImagePortName); + options.put("remoteDepthPort", remoteDepthPortName); + options.put("remoteRpcPort", remoteRpcPortName); + + PolyDriverDescriptor device(name, std::make_shared()); + + if (!device.poly->open(options) && !device.poly->isValid()) + { + log()->error("{} Could not open polydriver object.", errorPrefix); + return PolyDriverDescriptor(); + } + + return device; +} diff --git a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ICameraBridge.h b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ICameraBridge.h index ab9840fcfb..b143aa2cd8 100644 --- a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ICameraBridge.h +++ b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ICameraBridge.h @@ -136,6 +136,18 @@ class ICameraBridge return false; }; + /** + * Get the stored metadata. + * @return a const reference to the metadata + */ + virtual const CameraBridgeMetaData& getMetaData() const = 0; + + /** + * @brief Determines the validity of the object retrieved with getMetadata() + * @return True if the object is valid, false otherwise. + */ + virtual bool isValid() const = 0; + /** * Destructor */ @@ -195,12 +207,6 @@ class ICameraBridge { return true; }; - - /** - * Get the stored metadata. - * @return a const reference to the metadata - */ - virtual const CameraBridgeMetaData& getMetaData() const = 0; }; } // namespace RobotInterface } // namespace BipedalLocomotion