Camera bridge bindings

CHANGELOG.md

@@ -1,6 +1,14 @@
 # Changelog
 All notable changes to this project are documented in this file.
 
+## [unreleased]
+### Added
+- 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
+- Make `ICameraBridge::isValid()` virtual function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/695)
+
 ## [0.14.0] - 2023-07-04
 ### Added
 - Implement a class to perform inference with the [MANN network](https://github.com/ami-iit/mann-pytorch) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/652, https://github.com/ami-iit/bipedal-locomotion-framework/pull/686)

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 "$<BUILD_INTERFACE:${pybind_include_dirs}>")
+target_compile_definitions(pybind11_blf PRIVATE ${pybind_compile_definitions})
 
 target_link_libraries(pybind11_blf PRIVATE
   ${pybind_link_libraries})

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        e91b2f04e7b7e8c2858ef4d45b434591aa6c0e25
+      )
+    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()

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 <pybind11/pybind11.h>
+
+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

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 <pybind11/pybind11.h>
+
+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

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 <cvnp/cvnp.h>
+#endif // PYBIND_VERSION_AT_LEAST(2, 7, 0)
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/RobotInterface/ICameraBridge.h>
+#include <BipedalLocomotion/RobotInterface/YarpCameraBridge.h>
+#include <BipedalLocomotion/RobotInterface/YarpHelper.h>
+
+#include <BipedalLocomotion/System/Source.h>
+
+#include <BipedalLocomotion/bindings/RobotInterface/CameraBridge.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace RobotInterface
+{
+
+void CreateICameraBridge(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::RobotInterface;
+    using namespace BipedalLocomotion::ParametersHandler;
+
+    py::class_<CameraBridgeOptions>(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_<CameraLists>(module, "CameraLists")
+        .def(py::init())
+        .def_readwrite("rgb_cameras_list", &CameraLists::rgbCamerasList)
+        .def_readwrite("rgbd_cameras_list", &CameraLists::rgbdCamerasList);
+
+    py::class_<CameraBridgeMetaData>(module, "CameraBridgeMetaData")
+        .def(py::init())
+        .def_readwrite("sensors_list", &CameraBridgeMetaData::sensorsList)
+        .def_readwrite("bridge_options", &CameraBridgeMetaData::bridgeOptions);
+
+    py::class_<ICameraBridge> iCameraBridge(module, "ICameraBridge");
+    iCameraBridge
+        .def(
+            "initialize",
+            [](ICameraBridge& impl, std::shared_ptr<const IParametersHandler> 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<bool, cv::Mat> 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<bool, cv::Mat> 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_<YarpCameraBridge, ICameraBridge>(module, "YarpCameraBridge")
+        .def(py::init())
+        .def(
+            "set_drivers_list",
+            [](YarpCameraBridge& impl, std::vector<PolyDriverDescriptor> 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

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 <pybind11/pybind11.h>
+
+#include <BipedalLocomotion/bindings/RobotInterface/PerceptionModule.h>
+
+#include <BipedalLocomotion/bindings/RobotInterface/CameraBridge.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace RobotInterface
+{
+void CreatePerceptionModule(pybind11::module& module)
+{
+    CreateICameraBridge(module);
+    CreateYarpCameraBridge(module);
+}
+} // namespace RobotInterface
+} // namespace bindings
+} // namespace BipedalLocomotion

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<IParametersHandler> handler) -> PolyDriverDescriptor {
+        [](std::shared_ptr<const IParametersHandler> handler) -> PolyDriverDescriptor {
             return constructRemoteControlBoardRemapper(handler);
         },
         py::arg("handler"));
 
     module.def(
         "construct_generic_sensor_client",
-        [](std::shared_ptr<IParametersHandler> handler) -> PolyDriverDescriptor {
+        [](std::shared_ptr<const IParametersHandler> handler) -> PolyDriverDescriptor {
             return constructGenericSensorClient(handler);
         },
         py::arg("handler"));
 
     module.def(
         "construct_multiple_analog_sensors_client",
-        [](std::shared_ptr<IParametersHandler> handler) -> PolyDriverDescriptor {
+        [](std::shared_ptr<const IParametersHandler> handler) -> PolyDriverDescriptor {
             return constructMultipleAnalogSensorsClient(handler);
         },
         py::arg("handler"));
 
     module.def(
         "construct_multiple_analog_sensors_remapper",
-        [](std::shared_ptr<IParametersHandler> handler) -> PolyDriverDescriptor {
+        [](std::shared_ptr<const IParametersHandler> handler) -> PolyDriverDescriptor {
             return constructMultipleAnalogSensorsRemapper(handler);
         },
         py::arg("handler"));
+
+    module.def(
+        "construct_RGBD_sensor_client",
+        [](std::shared_ptr<const IParametersHandler> handler) -> PolyDriverDescriptor {
+            return constructRDGBSensorClient(handler);
+        },
+        py::arg("handler"));
 }
 
 } // namespace RobotInterface

bindings/python/bipedal_locomotion_framework.cpp.in

@@ -58,6 +58,10 @@
 #include <BipedalLocomotion/bindings/RobotInterface/Module.h>
 @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::RobotInterfaceYarpImplementation
 
+@cmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation
+#include <BipedalLocomotion/bindings/RobotInterface/PerceptionModule.h>
+@endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::PerceptionInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::PerceptionInterfaceYarpImplementation
+
 @cmakeiftarget BipedalLocomotion::FloatingBaseEstimators
 #include <BipedalLocomotion/bindings/FloatingBaseEstimators/Module.h>
 @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);

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()

src/Perception/include/BipedalLocomotion/Perception/Capture/RealSense.h

@@ -45,7 +45,11 @@ class RealSense : public BipedalLocomotion::RobotInterface::ICameraBridge,
     bool initialize(
         std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> 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<std::string>& rgbCamerasList) final;
 

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)
     {