Skip to content

Added logging of trintrin messages #1013

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
S-Dafarra wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Added logging of trintrin messages #1013

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
2ff1409
Added trintrin dependency
S-Dafarra Oct 24, 2025
4f52d98
Added possibility to log HumanState messages
S-Dafarra Oct 24, 2025
d7815c9
Convert HumanState to VectorsCollection to unify code with different …
S-Dafarra Oct 25, 2025
2fc0681
Added logging of WearableTargets message
S-Dafarra Oct 26, 2025
3843f9b
Added logging of WearableData
S-Dafarra Oct 26, 2025
65c4f9b
Added options to log wearable messages in conf files
S-Dafarra Oct 26, 2025
6bcb687
Moved message conversions to separate files
S-Dafarra Oct 26, 2025
9ecc663
Remove robotology channel
S-Dafarra Nov 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions .ci/blf_apt_deps.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,10 @@ repositories:
type: git
url: https://github.com/robotology/robometry.git
version: v1.2.6
trintrin:
type: git
url: https://github.com/ami-iit/trintrin.git
version: v0.0.1
bayes-filters-lib:
type: git
url: https://github.com/robotology/bayes-filters-lib.git
Expand Down
2 changes: 1 addition & 1 deletion .github/workflows/conda-forge-ci.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ jobs:
"idyntree>=12.2.1" "yarp>=3.5.0" libmatio libmatio-cpp librobometry \
liblie-group-controllers eigen qhull "casadi>=3.5.5" cppad "spdlog>=1.15.1" \
nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl \
tomlplusplus libunicycle-footstep-planner "icub-models>=1.23.4" \
tomlplusplus libunicycle-footstep-planner "icub-models>=1.23.4" libtrintrin \
ros-jazzy-rclcpp onnxruntime-cpp libbayes-filters-lib cmake-package-check catch2

- name: Linux-only Dependencies [Linux]
Expand Down
5 changes: 4 additions & 1 deletion cmake/BipedalLocomotionFrameworkDependencies.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,9 @@ checkandset_dependency(UnicyclePlanner)
blf_optional_find_package(onnxruntime QUIET)
checkandset_dependency(onnxruntime)

blf_optional_find_package(trintrin QUIET)
checkandset_dependency(trintrin)

########################## Test-related options ##############################

# MemoryAllocationMonitor require glibc >= 2.35
Expand Down Expand Up @@ -279,7 +282,7 @@ 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" OFF)
"FRAMEWORK_COMPILE_RobotInterface;FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Perception;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
Expand Down
5 changes: 3 additions & 2 deletions devices/YarpRobotLoggerDevice/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,14 +43,15 @@ if(FRAMEWORK_COMPILE_YarpRobotLoggerDevice)
add_bipedal_yarp_device(
NAME YarpRobotLoggerDevice
TYPE BipedalLocomotion::YarpRobotLoggerDevice
SOURCES src/YarpRobotLoggerDevice.cpp src/YarpTextLoggingUtilities.cpp
PUBLIC_HEADERS include/BipedalLocomotion/YarpRobotLoggerDevice.h include/BipedalLocomotion/YarpTextLoggingUtilities.h
SOURCES src/YarpRobotLoggerDevice.cpp src/YarpTextLoggingUtilities.cpp src/MessageConversionUtilities.cpp
PUBLIC_HEADERS include/BipedalLocomotion/YarpRobotLoggerDevice.h include/BipedalLocomotion/YarpTextLoggingUtilities.h include/BipedalLocomotion/MessageConversionUtilities.h
PRIVATE_LINK_LIBRARIES
Eigen3::Eigen
YARP::YARP_os
YARP::YARP_dev
YARP::YARP_profiler
robometry::robometry
trintrin::msgs
BipedalLocomotion::TextLogging
BipedalLocomotion::System
BipedalLocomotion::YarpUtilities
Expand Down
5 changes: 4 additions & 1 deletion devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,11 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<!--group name="Balancing">
<!--group name="Balancing">
<param name="local">"/yarp-robot-logger/exogenous_signals/balancing:i"</param>
<param name="remote">"/balancing_controller/logger/data:o"</param>
<param name="signal_name">"balancing"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
5 changes: 5 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000_spu01/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,11 @@ BSD-3-Clause license. -->
<group name="ExogenousSignals">
<param name="vectors_collection_exogenous_inputs">("Walking", "amp", "current_commands", "balancing", "force_platforms", TorqueControlTracking, joint_control)</param>
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
3 changes: 3 additions & 0 deletions devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ BSD-3-Clause license. -->
<param name="vectors_exogenous_inputs">()</param>
<param name="string_exogenous_inputs">()</param>
<param name="image_exogenous_inputs">()</param>
<param name="human_state_exogenous_inputs">()</param>
<param name="wearable_targets_exogenous_inputs">()</param>
<param name="wearable_data_exogenous_inputs">()</param>

<group name="Walking">
<param name="local">"/yarp-robot-logger/exogenous_signals/walking"</param>
Expand Down
45 changes: 45 additions & 0 deletions devices/YarpRobotLoggerDevice/include/BipedalLocomotion/MessageConversionUtilities.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
/**
* @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_MESSAGE_CONVERSION_UTILITIES_H
#define BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_MESSAGE_CONVERSION_UTILITIES_H

#include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
#include <BipedalLocomotion/YarpUtilities/VectorsCollectionMetadata.h>

#include <trintrin/msgs/HumanState.h>
#include <trintrin/msgs/WearableTargets.h>
#include <trintrin/msgs/WearableData.h>

#include <string>

namespace BipedalLocomotion
{

static const std::string treeDelim = "::";

void extractMetadata(const trintrin::msgs::HumanState& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata& metadata);
void extractMetadata(const trintrin::msgs::WearableTargets& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata& metadata);
void extractMetadata(const trintrin::msgs::WearableData& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata& metadata);

void convertToVectorsCollection(const trintrin::msgs::HumanState& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollection& collection);
void convertToVectorsCollection(const trintrin::msgs::WearableTargets& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollection& collection);
void convertToVectorsCollection(const trintrin::msgs::WearableData& message,
const std::string& prefix,
BipedalLocomotion::YarpUtilities::VectorsCollection& collection);
Comment on lines +23 to +41
Copy link
Collaborator

@GiulioRomualdi GiulioRomualdi Dec 4, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could you add the documentation here?


}

#endif // BIPEDAL_LOCOMOTION_FRAMEWORK_YARP_ROBOT_LOGGER_DEVICE_MESSAGE_CONVERSION_UTILITIES_H
17 changes: 16 additions & 1 deletion devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@

#include <robometry/BufferManager.h>

#include <trintrin/msgs/HumanState.h>
#include <trintrin/msgs/WearableTargets.h>
#include <trintrin/msgs/WearableData.h>

#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/RobotInterface/YarpCameraBridge.h>
#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
Expand Down Expand Up @@ -117,11 +121,23 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
void disconnect();
};

template <typename T> struct ExogenousSignalWithMetadata : ExogenousSignal<T>
{
BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata metadata;
BipedalLocomotion::YarpUtilities::VectorsCollection convertedSignal;
};

std::unordered_map<std::string, VectorsCollectionSignal> m_vectorsCollectionSignals;
std::unordered_map<std::string, ExogenousSignal<yarp::sig::Vector>> m_vectorSignals;
std::unordered_map<std::string, ExogenousSignal<yarp::os::Bottle>> m_stringSignals;
std::unordered_map<std::string, ExogenousSignal<yarp::sig::ImageOf<yarp::sig::PixelRgb>>>
m_imageSignals;
std::unordered_map<std::string, ExogenousSignalWithMetadata<trintrin::msgs::HumanState>>
m_humanStateSignals;
std::unordered_map<std::string, ExogenousSignalWithMetadata<trintrin::msgs::WearableTargets>>
m_wearableTargetsSignals;
std::unordered_map<std::string, ExogenousSignalWithMetadata<trintrin::msgs::WearableData>>
m_wearableDataSignals;

std::atomic<bool> m_lookForNewExogenousSignalIsRunning{false};
std::thread m_lookForNewExogenousSignalThread;
Expand Down Expand Up @@ -268,7 +284,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
bool prepareRTStreaming();

const std::string defaultFilePrefix = "robot_logger_device";
const std::string treeDelim = "::";

const std::string robotRtRootName = "robot_realtime";

Expand Down
Loading
Loading