Skip to content
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

Add to TelemetryDeviceDumper streaming of rawValues from low level #190

Merged
merged 3 commits into from
Nov 7, 2024
Merged

Add to TelemetryDeviceDumper streaming of rawValues from low level #190

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bee1486
Update telemetry for rawValuesPublisher
MSECode Sep 4, 2024
a99ef45
Update namespace, interface API and if loop
MSECode Oct 22, 2024
25892f8
Update README.md and required icub-main v2.7.0 for compiling Telemetr…
MSECode Nov 1, 2024
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
6 changes: 4 additions & 2 deletions src/telemetryDeviceDumper/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,14 @@ if(ENABLE_telemetryDeviceDumper)
add_definitions(-D_USE_MATH_DEFINES)
endif()

find_package(iCubDev REQUIRED)
Copy link
Member

Choose a reason for hiding this comment

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

Have you bumped the patch version of icub-main? I would put here the required version that includes the new interface

Copy link
Contributor Author

@MSECode MSECode Oct 31, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

Yes, actually as suggested by @pattacini we bumped the minor version, current version is 2.7.0

https://github.com/robotology/icub-main/blob/devel/CMakeLists.txt

Copy link
Member

Choose a reason for hiding this comment

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

Great! It should be then find_package(iCubDev 2.7.0 REQUIRED), and I would also add it in the readme documentation of the device that that version of icub-main is required

Copy link
Contributor Author

Choose a reason for hiding this comment

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

done

target_sources(yarp_telemetryDeviceDumper PRIVATE TelemetryDeviceDumper.cpp
TelemetryDeviceDumper.h)

target_link_libraries(yarp_telemetryDeviceDumper PRIVATE YARP::YARP_os
YARP::YARP_dev
robometry::robometry)
robometry::robometry
ICUB::iCubDev)
list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os
YARP_dev
robometry)
Expand Down
81 changes: 76 additions & 5 deletions src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
using namespace robometry;
using namespace yarp::os;
using namespace yarp::dev;
using namespace iCub;

constexpr double log_thread_default{ 0.010 };

Expand Down Expand Up @@ -117,6 +118,12 @@ bool TelemetryDeviceDumper::loadSettingsFromConfig(yarp::os::Searchable& config)
settings.logILocalization2D = prop.find(logILocalization2DOptionName.c_str()).asBool();
}

std::string logIRawValuesPublisherOptionName = "logIRawValuesPublisher";
if (prop.check(logIRawValuesPublisherOptionName.c_str()))
{
settings.logIRawValuesPublisher = prop.find(logIRawValuesPublisherOptionName.c_str()).asBool();
}

std::string useRadians = "useRadians";
if (prop.check(useRadians.c_str())) {
settings.useRadians = prop.find(useRadians.c_str()).asBool();
Expand All @@ -132,6 +139,11 @@ bool TelemetryDeviceDumper::loadSettingsFromConfig(yarp::os::Searchable& config)
settings.localizationRemoteName = prop.find(localizationRemoteName.c_str()).asString();
}

std::string rawValuesPublisherRemoteName = "rawValuesPublisherRemoteName";
if (prop.check(rawValuesPublisherRemoteName.c_str()) && prop.find(rawValuesPublisherRemoteName.c_str()).isString()) {
settings.rawValuesPublisherRemoteName = prop.find(rawValuesPublisherRemoteName.c_str()).asString();
}

// BufferManager options
std::string json_file = "json_file";
if (prop.check(json_file.c_str()) && prop.find(json_file.c_str()).isString()) {
Expand Down Expand Up @@ -229,7 +241,7 @@ bool TelemetryDeviceDumper::open(yarp::os::Searchable& config) {
if (settings.logILocalization2D) {
yarp::os::Property loc2DClientProp{{"device", Value("localization2DClient")},
{"remote", Value(settings.localizationRemoteName)},
{"local", Value("/telemetryDeviceDumper" + settings.localizationRemoteName + "/client")}};;
{"local", Value("/telemetryDeviceDumper" + settings.localizationRemoteName + "/client")}};
ok = this->localization2DClient.open(loc2DClientProp);
ok = ok && this->localization2DClient.view(iloc);
if (!ok) {
Expand All @@ -246,6 +258,20 @@ bool TelemetryDeviceDumper::open(yarp::os::Searchable& config) {
return false;
}

// Open RawValuesPublisherClient
if (settings.logIRawValuesPublisher)
{
yarp::os::Property rawValPubClientProp{{"device", Value("rawValuesPublisherClient")},
{"remote", Value(settings.rawValuesPublisherRemoteName)}, //must have the name of the remote port defined in the related nws, i.e. RawValuesPublisherServer
{"local", Value("/telemetryDeviceDumper" + settings.rawValuesPublisherRemoteName + "/client")}};
ok = this->rawValuesPublisherClient.open(rawValPubClientProp);
ok = ok && this->rawValuesPublisherClient.view(iravap);
if (!ok) {
yError() << "telemetryDeviceDumper: Problem opening the rawValuesPublisherClient.";
return false;
}
}

ok = this->configBufferManager(config);
if (!ok)
{
Expand Down Expand Up @@ -309,10 +335,12 @@ bool TelemetryDeviceDumper::openRemapperControlBoard(yarp::os::Searchable& confi

int axes = 0;
ok = ok && remappedControlBoardInterfaces.encs->getAxes(&axes);
if (ok) {
if (ok)
{
this->resizeBuffers(axes);
}
else {
else
{
yError() << "telemetryDeviceDumper: open impossible to use the necessary interfaces in remappedControlBoard";
return ok;
}
Expand Down Expand Up @@ -363,6 +391,7 @@ void TelemetryDeviceDumper::resizeBuffers(int size) {
this->interactionModes.resize(size);
// OdometryData has 9 fields
this->odometryData.resize(9);
this->rawDataValuesMap.clear();

}

Expand Down Expand Up @@ -412,7 +441,6 @@ bool TelemetryDeviceDumper::configBufferManager(yarp::os::Searchable& conf) {
ok = ok && bufferManager.addChannel({ "odometry_data", {odometryData.size(), 1}, m_bufferConfig.description_list });
}

ok = ok && bufferManager.configure(m_bufferConfig);
// TODO check if we have nr of channels ~= 0
return ok;
}
Expand All @@ -423,6 +451,19 @@ bool TelemetryDeviceDumper::attachAll(const yarp::dev::PolyDriverList& device2at
bool ok = true;
ok = ok && this->attachAllControlBoards(device2attach);

if (ok && (settings.logIRawValuesPublisher))
{
// Configuring channels using metadata from interfaces
rawValuesKeyMetadataMap metadata = {}; // I just need to call it once while configuring (I think)
iravap->getMetadataMap(metadata);
for (auto [k, m] : metadata.metadataMap)
{
ok = ok && bufferManager.addChannel({ "raw_data_values::"+k, {static_cast<uint16_t>(m.size), 1}, m.rawValueNames });
}
}

ok = ok && bufferManager.configure(m_bufferConfig);

if (ok)
{
correctlyConfigured = true;
Expand Down Expand Up @@ -450,6 +491,12 @@ bool TelemetryDeviceDumper::close()
localization2DClient.close();
}

if (settings.logIRawValuesPublisher)
{
rawValuesPublisherClient.close();
}


bool ok = true;
if (settings.saveBufferManagerConfiguration) {
auto buffConfToSave = bufferManager.getBufferConfig();
Expand Down Expand Up @@ -692,7 +739,7 @@ void TelemetryDeviceDumper::readOdometryData() {
ok = iloc->getEstimatedOdometry(yarpOdomData);
if (!ok)
{
yWarning() << "telemetryDeviceDumper warning : odometry_data was not readed correctly";
yWarning() << "telemetryDeviceDumper warning : odometry_data was not read correctly";
}
else
{
Expand All @@ -703,12 +750,36 @@ void TelemetryDeviceDumper::readOdometryData() {
}
}

void TelemetryDeviceDumper::readRawValuesData()
{
bool ok;
ok = iravap->getRawDataMap(rawDataValuesMap);
if (!ok)
{
yWarning() << "telemetryDeviceDumper warning : raw_data_values was not read correctly";
}
else
{
for (auto [key,value] : rawDataValuesMap)
{
bufferManager.push_back(value, "raw_data_values::"+key);
}
}

}

void TelemetryDeviceDumper::run() {
if (correctlyConfigured) {
readSensors();
if (settings.logILocalization2D) {
readOdometryData();
}

if (settings.logIRawValuesPublisher)
{
readRawValuesData();
}

}
return;
}
Expand Down
18 changes: 16 additions & 2 deletions src/telemetryDeviceDumper/TelemetryDeviceDumper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <yarp/dev/IControlMode.h>
#include <yarp/dev/IInteractionMode.h>
#include <yarp/dev/ILocalization2D.h>
#include <iCub/IRawValuesPublisher.h>
#include <yarp/dev/IMotor.h>
#include <yarp/dev/ITorqueControl.h>
#include <yarp/dev/PolyDriver.h>
Expand All @@ -27,6 +28,7 @@
#include <robometry/BufferManager.h>

#include <unordered_map>
#include <map>
#include <string>
#include <memory>
#include <vector>
Expand All @@ -47,9 +49,12 @@ struct TelemetryDeviceDumperSettings {
bool logIPidControl{ false };
bool logIAmplifierControl{ false };
bool logILocalization2D{ false };
bool logIRawValuesPublisher { false };
bool useRadians{ false };
bool saveBufferManagerConfiguration{ false };
std::string localizationRemoteName{ "" };
std::string rawValuesPublisherRemoteName { "" };

};
/**
* @brief The `telemetryDeviceDumper` is a [yarp device](http://yarp.it/git-master/note_devices.html)
Expand All @@ -69,6 +74,7 @@ struct TelemetryDeviceDumperSettings {
* | `logIAmplifierControl` | bool | - | false | No | Enable the log of `motors_state::pwm` and `motors_state::currents` (http://yarp.it/git-master/classyarp_1_1dev_1_1IAmplifierControl.html). |
* | `logControlBoardQuantities` | bool | - | false | No | Enable the log of all the quantities that requires the attach to a control board (`logIEncoders`, `logITorqueControl`, `logIMotorEncoders`, `logIControlMode`, `logIInteractionMode`, `logIPidControl`, `logIAmplifierControl`). |
* | `logILocalization2D` | bool | - | false | No | Enable the log of `odometry_data` (http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html). |
* | `logIRawValuesPublisher` | bool | - | false | No | Enable the log of raw values |
Nicogene marked this conversation as resolved.
Show resolved Hide resolved
* | `saveBufferManagerConfiguration` | bool | - | false | No | Enable the save of the configuration of the BufferManager into `path`+ `"bufferConfig"` + `experimentName` + `".json"` |
* | `json_file` | string | - | - | No | Configure the `robometry::BufferManager`s reading from a json file like [in Example configuration file](#example-configuration-file). Note that this configuration will overwrite the parameter-by-parameter configuration |
* | `experimentName` | string | - | - | Yes | Prefix of the files that will be saved. The files will be named: `experimentName`+`timestamp`+ `".mat"`. |
Expand Down Expand Up @@ -99,7 +105,8 @@ struct TelemetryDeviceDumperSettings {
* | `PIDs::torque_error` | [`yarp::dev::IPidControl::getPidErrors`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#aea29e0fdf34f819ac69a3b940556ba28) |
* | `PIDs::torque_reference` | [`yarp::dev::IPidControl::getPidReferences`](http://yarp.it/git-master/classyarp_1_1dev_1_1IPidControl.html#a29e8f684a15d859229a9ae2902f886da) |
* | `PIDs::odometry_data ` | [`yarp::dev::Nav2D::ILocalization2D::getEstimatedOdometry`](http://yarp.it/git-master/classyarp_1_1dev_1_1Nav2D_1_1ILocalization2D.html#a02bff57282777ce7511b671abd4c95f0) |
*
* | `raw_data_values` | [`iCub::debugLibrary::IRawValuesPublisher::getRawDataMap`]
Nicogene marked this conversation as resolved.
Show resolved Hide resolved
*
* @section Example_xml Example of xml
*
* Example of xml file for using it on the `iCub` robot:
Expand All @@ -118,6 +125,7 @@ struct TelemetryDeviceDumperSettings {
* <param name="logIInteractionMode">true</param>
* <param name="logIPidControl">false</param>
* <param name="logIAmplifierControl">true</param>
* <param name="logIRawValuesPublisher">false</param>
* <param name="saveBufferManagerConfiguration">true</param>
* <param name="experimentName">test_telemetry</param>
* <param name="path">/home/icub/test_telemetry/</param>
Expand Down Expand Up @@ -182,10 +190,11 @@ class TelemetryDeviceDumper : public yarp::dev::DeviceDriver,
bool openRemapperControlBoard(yarp::os::Searchable& config);
void readSensors();
void readOdometryData();
void readRawValuesData();
void resizeBuffers(int size);
bool configBufferManager(yarp::os::Searchable& config);
/** Remapped controlboard containg the axes for which the joint torques are estimated */
yarp::dev::PolyDriver remappedControlBoard, localization2DClient;
yarp::dev::PolyDriver remappedControlBoard, localization2DClient, rawValuesPublisherClient;
struct
{
yarp::dev::IEncoders* encs{nullptr};
Expand All @@ -200,12 +209,17 @@ class TelemetryDeviceDumper : public yarp::dev::DeviceDriver,

yarp::dev::Nav2D::ILocalization2D* iloc{nullptr};

iCub::debugLibrary::IRawValuesPublisher* iravap{ nullptr };

std::mutex deviceMutex;
std::atomic<bool> correctlyConfigured{ false }, sensorsReadCorrectly{false};
std::vector<double> jointPos, jointVel, jointAcc, jointPosErr, jointPosRef,
jointTrqErr, jointTrqRef, jointPWM, jointCurr, jointTrq,
motorEnc, motorVel, motorAcc, controlModes, interactionModes,
odometryData;

std::map<std::string, std::vector<std::int32_t>> rawDataValuesMap;

std::vector<std::string> jointNames;
TelemetryDeviceDumperSettings settings;
robometry::BufferConfig m_bufferConfig;
Expand Down