feat: Add auto-start logging feature and refactor device state management

README.md

@@ -58,7 +58,8 @@ The **bipedal-locomotion-framework** project consists of several components. The
 |              [`Math`](./src/Math)              |          Library containing mathematical algorithms          |      [`manif`](https://github.com/artivis/manif)             |
 |              [`ML`](./src/ML)              |          Library containing deep learning algorithms           |      [`onnxruntime`](https://onnxruntime.ai/)             |
 | [`ParametersHandler`](./src/ParametersHandler) |  Library for retrieving parameters from configuration files  | [`YARP`](https://www.yarp.it/git-master/) (only if you want the `YARP` implementation) [`tomlplusplus`](https://github.com/marzer/tomlplusplus/) (only if you want the `toml` implementation) |
-| [`PerceptionInterface`](./src/RobotInterface)  | Generic interface classes to adapt to perception data formats like images and point clouds | [`OpenCV`](https://github.com/opencv/opencv) [`PCL`](https://github.com/PointCloudLibrary/pcl) |
+| [`CameraInterface`](./src/RobotInterface)  | Generic interface classes to adapt to image data formats | [`OpenCV`](https://github.com/opencv/opencv) |
+| [`PointCloudInterface`](./src/RobotInterface)  | Generic interface classes to adapt to point-cloud data formats | [`PCL`](https://github.com/PointCloudLibrary/pcl) |
 |    [`PerceptionCapture`](./src/Perception)     |   Library containing driver classes for perception devices   | [`OpenCV`](https://github.com/opencv/opencv) [`PCL`](https://github.com/PointCloudLibrary/pcl) [`realsense2`](https://github.com/IntelRealSense/librealsense) |
 |    [`PerceptionFeatures`](./src/Perception)     |   Library containing perception algorithms useful for locomotion   | [`OpenCV`](https://github.com/opencv/opencv) |
 |          [`Planners`](./src/Planners)          |       Library containing planner useful for locomotion       | [`manif`](https://github.com/artivis/manif) [`CasADi`](https://web.casadi.org/) [`qhull`](http://www.qhull.org/) |

bindings/python/RobotInterface/CMakeLists.txt

@@ -27,9 +27,9 @@ if(TARGET BipedalLocomotion::RobotInterface AND TARGET BipedalLocomotion::RobotI
 endif()
 
 if(TARGET BipedalLocomotion::RobotInterface
-    AND TARGET BipedalLocomotion::PerceptionInterface
+  AND TARGET BipedalLocomotion::CameraInterface
     AND TARGET BipedalLocomotion::RobotInterfaceYarpImplementation
-    AND TARGET BipedalLocomotion::PerceptionInterfaceYarpImplementation)
+  AND TARGET BipedalLocomotion::CameraInterfaceYarpImplementation)
 
   if(FRAMEWORK_COMPILE_PYTHON_BINDINGS)
     set(cvnp_target_link )
@@ -54,10 +54,10 @@ if(TARGET BipedalLocomotion::RobotInterface
   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}
+    NAME CameraInterfaceBindings
+    SOURCES src/CameraModule.cpp src/CameraBridge.cpp
+    HEADERS ${H_PREFIX}/CameraModule.h ${H_PREFIX}/CameraBridge.h
+    LINK_LIBRARIES BipedalLocomotion::CameraInterface BipedalLocomotion::CameraInterfaceYarpImplementation ${cvnp_target_link}
     ADDITIONAL_COMPILE_DEFINITIONS pybind11_VERSION_MAJOR=${pybind11_VERSION_MAJOR}
                                    pybind11_VERSION_MINOR=${pybind11_VERSION_MINOR}
                                    pybind11_VERSION_PATCH=${pybind11_VERSION_PATCH}

bindings/python/RobotInterface/include/BipedalLocomotion/bindings/RobotInterface/CameraModule.h

@@ -0,0 +1,25 @@
+/**
+ * @file CameraModule.h
+ * @authors Giulio Romualdi
+ * @copyright Generative Bionics S.R.L. This software may be modified and distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_MODULE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace RobotInterface
+{
+
+void CreateCameraModule(pybind11::module& module);
+
+} // namespace RobotInterface
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_INTERFACE_CAMERA_MODULE_H

bindings/python/RobotInterface/src/PerceptionModule.cpp → bindings/python/RobotInterface/src/CameraModule.cpp

@@ -1,13 +1,12 @@
 /**
- * @file PerceptionModule.cpp
+ * @file CameraModule.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.
+ * @copyright Generative Bionics S.R.L. 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/CameraModule.h>
 
 #include <BipedalLocomotion/bindings/RobotInterface/CameraBridge.h>
 
@@ -17,7 +16,7 @@ namespace bindings
 {
 namespace RobotInterface
 {
-void CreatePerceptionModule(pybind11::module& module)
+void CreateCameraModule(pybind11::module& module)
 {
     CreateICameraBridge(module);
     CreateYarpCameraBridge(module);

bindings/python/bipedal_locomotion_framework.cpp.in

@@ -3,6 +3,7 @@
  * @authors Giulio Romualdi, Diego Ferigo
  * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
  * distributed under the terms of the BSD-3-Clause license.
+ * @copyright Generative Bionics S.R.L. This software may be modified and distributed under the terms of the BSD-3-Clause license.
  */
 
 // clang-format off
@@ -61,9 +62,9 @@
 #include <BipedalLocomotion/bindings/RobotInterface/YarpModule.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::RobotInterface && BipedalLocomotion::CameraInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::CameraInterfaceYarpImplementation
+#include <BipedalLocomotion/bindings/RobotInterface/CameraModule.h>
+@endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::CameraInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::CameraInterfaceYarpImplementation
 
 @cmakeiftarget BipedalLocomotion::FloatingBaseEstimators
 #include <BipedalLocomotion/bindings/FloatingBaseEstimators/Module.h>
@@ -183,9 +184,9 @@ PYBIND11_MODULE(bindings, m)
     bindings::RobotInterface::CreateYarpModule(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::RobotInterface && BipedalLocomotion::CameraInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::CameraInterfaceYarpImplementation
+    bindings::RobotInterface::CreateCameraModule(robotInterfaceModule);
+    @endcmakeiftarget BipedalLocomotion::RobotInterface && BipedalLocomotion::CameraInterface && BipedalLocomotion::RobotInterfaceYarpImplementation && BipedalLocomotion::CameraInterfaceYarpImplementation
 
     @cmakeiftarget BipedalLocomotion::FloatingBaseEstimators
     py::module floatingBaseEstimatorModule = m.def_submodule("floating_base_estimators");

cmake/BipedalLocomotionFrameworkDependencies.cmake

@@ -97,9 +97,9 @@ blf_optional_find_package(tomlplusplus 3.0.1 QUIET)
 checkandset_dependency(tomlplusplus MINIMUM_VERSION 3.0.1)
 dependency_classifier(tomlplusplus MINIMUM_VERSION 3.0.1 IS_USED ${FRAMEWORK_USE_tomlplusplus} PUBLIC)
 
-blf_optional_find_package(robometry 1.1.0 QUIET)
-checkandset_dependency(robometry MINIMUM_VERSION 1.1.0)
-dependency_classifier(robometry MINIMUM_VERSION 1.1.0 IS_USED ${FRAMEWORK_USE_robometry})
+blf_optional_find_package(robometry 1.4.1 QUIET)
+checkandset_dependency(robometry MINIMUM_VERSION 1.4.1)
+dependency_classifier(robometry MINIMUM_VERSION 1.4.1 IS_USED ${FRAMEWORK_USE_robometry})
 
 blf_optional_find_package(BayesFilters QUIET)
 checkandset_dependency(BayesFilters)
@@ -256,21 +256,21 @@ framework_dependent_option(FRAMEWORK_COMPILE_JointTrajectoryPlayer
   "Compile joint-trajectory-player application?" ON
   "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_matioCppConversions;FRAMEWORK_USE_matioCpp;FRAMEWORK_USE_YARP;FRAMEWORK_COMPILE_Contact" OFF)
 
-framework_dependent_option(FRAMEWORK_COMPILE_Perception
-  "Compile Perception libraries?" ON
-  "FRAMEWORK_USE_OpenCV;FRAMEWORK_USE_PCL" OFF)
+framework_dependent_option(FRAMEWORK_COMPILE_Camera
+  "Compile camera-related libraries?" ON
+  "FRAMEWORK_USE_OpenCV" OFF)
 
-framework_dependent_option(FRAMEWORK_COMPILE_PerceptionInterface
-  "Compile PerceptionInterface libraries?" ON
-  "FRAMEWORK_COMPILE_Perception" OFF)
+framework_dependent_option(FRAMEWORK_COMPILE_Pointcloud
+  "Compile pointcloud-related libraries?" ON
+  "FRAMEWORK_USE_PCL" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_RealsenseCapture
   "Compile Realsense related software?" ON
-  "FRAMEWORK_COMPILE_PerceptionInterface;FRAMEWORK_USE_realsense2" OFF)
+  "FRAMEWORK_COMPILE_Camera;FRAMEWORK_COMPILE_Pointcloud;FRAMEWORK_USE_realsense2" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_RealSenseTestApplication
   "Compile realsense-test application?" ON
-  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;FRAMEWORK_COMPILE_RealsenseCapture;FRAMEWORK_COMPILE_PerceptionInterface" OFF)
+  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Camera;FRAMEWORK_COMPILE_Pointcloud;FRAMEWORK_COMPILE_RealsenseCapture" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_CalibrationDeltaUpdaterApplication
   "Compile calibration-delta-updater application?" ON
@@ -282,11 +282,11 @@ framework_dependent_option(FRAMEWORK_COMPILE_BalancingPositionControlApplication
 
 framework_dependent_option(FRAMEWORK_COMPILE_YarpRobotLoggerDevice
   "Do you want to generate and compile the YarpRobotLoggerDevice?" ON
-  "FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_robometry;FRAMEWORK_USE_trintrin" OFF)
+  "FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Camera;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_robometry;FRAMEWORK_USE_trintrin" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_JointTorqueControlDevice
   "Do you want to generate and compile the YarpRobotLoggerDevice?" ON
-  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_onnxruntime" OFF)
+  "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_Math;FRAMEWORK_COMPILE_ContinuousDynamicalSystem;FRAMEWORK_USE_onnxruntime" OFF)
 
 framework_dependent_option(FRAMEWORK_COMPILE_VectorsCollectionWrapper
   "Do you want to generate and compile the VectorsCollectionWrapper?" ON

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -33,7 +33,11 @@ include(CMakeDependentOption)
 # is not defined (in that case find_package will be called by checkandset_dependency)
 macro(blf_optional_find_package package)
   if(NOT DEFINED FRAMEWORK_USE_{package})
+    set(BLF_CMAKE_MODULE_PATH_BACKUP "${CMAKE_MODULE_PATH}")
     find_package(${package} ${ARGN})
+    list(APPEND CMAKE_MODULE_PATH ${BLF_CMAKE_MODULE_PATH_BACKUP})
+    list(REMOVE_DUPLICATES CMAKE_MODULE_PATH)
+    unset(BLF_CMAKE_MODULE_PATH_BACKUP)
   endif()
 endmacro()
 
@@ -60,11 +64,15 @@ macro(checkandset_dependency package)
   if (${PREFIX}_HAS_SYSTEM_${package})
       mark_as_advanced(${PREFIX}_USE_${package})
   elseif (${PREFIX}_USE_${package})
+    set(BLF_CMAKE_MODULE_PATH_BACKUP "${CMAKE_MODULE_PATH}")
     if (CSD_COMPONENTS)
       find_package(${package} ${CSD_MINIMUM_VERSION} COMPONENTS ${CSD_COMPONENTS} REQUIRED)
     else ()
       find_package(${package} ${CSD_MINIMUM_VERSION} REQUIRED)
     endif ()
+    list(APPEND CMAKE_MODULE_PATH ${BLF_CMAKE_MODULE_PATH_BACKUP})
+    list(REMOVE_DUPLICATES CMAKE_MODULE_PATH)
+    unset(BLF_CMAKE_MODULE_PATH_BACKUP)
   endif()
 
   # FRAMEWORK_USE_SYSTEM_${package}

cmake/valgrind-linux.supp

@@ -581,3 +581,19 @@
    obj:/usr/lib/x86_64-linux-gnu/libtbb.so.12.11
    fun:_ZN2cv13parallel_for_ERKNS_5RangeERKNS_16ParallelLoopBodyEd
 }
+
+{
+   <ubuntu-24-04-aruco-marker-memcheck>
+   Memcheck:Leak
+   match-leak-kinds: possible
+   fun:calloc
+   fun:calloc
+   fun:allocate_dtv
+   fun:_dl_allocate_tls
+   fun:allocate_stack
+   fun:pthread_create@@GLIBC_2.34
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.12.11
+   obj:/usr/lib/x86_64-linux-gnu/libtbb.so.12.11
+   fun:start_thread
+   fun:clone
+}

devices/YarpRobotLoggerDevice/CMakeLists.txt

@@ -58,7 +58,7 @@ if(FRAMEWORK_COMPILE_YarpRobotLoggerDevice)
       BipedalLocomotion::VectorsCollection
       BipedalLocomotion::ParametersHandlerYarpImplementation
       BipedalLocomotion::RobotInterfaceYarpImplementation
-      BipedalLocomotion::PerceptionInterfaceYarpImplementation
+      BipedalLocomotion::CameraInterfaceYarpImplementation
       BipedalLocomotion::TextLoggingYarpImplementation
       BipedalLocomotion::SystemYarpImplementation
       BipedalLocomotion::YarpRobotLoggerDeviceCommands

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml

@@ -15,6 +15,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml

@@ -15,6 +15,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml

@@ -22,6 +22,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml

@@ -22,6 +22,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml

@@ -22,6 +22,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN003/yarp-robot-logger.xml

@@ -20,6 +20,7 @@ BSD-3-Clause license. -->
   <param name="log_text">true</param>
   <param name="text_logging_subnames">("ergocub-torso/yarprobotinterface")</param>
   <param name="code_status_cmds">("ssh ergocub@10.0.0.2 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_0_2.txt" "ssh ergocub@10.0.0.2 'apt list --installed' > {filename}_apt_10_0_0_2.txt" "ssh ergocub@10.0.0.3 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_0_3.txt" "ssh ergocub@10.0.0.3 'apt list --installed' > {filename}_apt_10_0_0_3.txt")</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml

@@ -12,6 +12,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">

devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml

@@ -14,6 +14,7 @@ BSD-3-Clause license. -->
   <param name="log_cameras">true</param>
   <param name="log_frames">false</param>
   <param name="log_robot_data">true</param>
+  <param name="auto_start_logging">true</param>
 
   <param name="enable_real_time_logging" extern-name="enable_real_time_logging">false</param>
   <group name="REAL_TIME_STREAMING">