diff --git a/src/telemetryDeviceDumper/CMakeLists.txt b/src/telemetryDeviceDumper/CMakeLists.txt index bb15fbf..ace36fa 100644 --- a/src/telemetryDeviceDumper/CMakeLists.txt +++ b/src/telemetryDeviceDumper/CMakeLists.txt @@ -20,12 +20,14 @@ if(ENABLE_telemetryDeviceDumper) add_definitions(-D_USE_MATH_DEFINES) endif() + find_package(iCubDev REQUIRED) 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) diff --git a/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp b/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp index 9b3d1d2..3d4846b 100644 --- a/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp +++ b/src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp @@ -117,6 +117,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(); @@ -132,6 +138,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()) { @@ -229,7 +240,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) { @@ -246,6 +257,19 @@ 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) { @@ -308,13 +332,15 @@ bool TelemetryDeviceDumper::openRemapperControlBoard(yarp::os::Searchable& confi } int axes = 0; - ok = ok && remappedControlBoardInterfaces.encs->getAxes(&axes); - if (ok) { - this->resizeBuffers(axes); - } - else { - yError() << "telemetryDeviceDumper: open impossible to use the necessary interfaces in remappedControlBoard"; - return ok; + if (settings.logControlBoardQuantities){ + ok = ok && remappedControlBoardInterfaces.encs->getAxes(&axes); + if (ok) { + this->resizeBuffers(axes); + } + else { + yError() << "telemetryDeviceDumper: open impossible to use the necessary interfaces in remappedControlBoard"; + return ok; + } } return true; @@ -363,6 +389,7 @@ void TelemetryDeviceDumper::resizeBuffers(int size) { this->interactionModes.resize(size); // OdometryData has 9 fields this->odometryData.resize(9); + this->rawDataValuesMap.clear(); } @@ -412,7 +439,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; } @@ -423,6 +449,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(m.size), 1}, m.rawValueNames }); + } + } + + ok = ok && bufferManager.configure(m_bufferConfig); + if (ok) { correctlyConfigured = true; @@ -450,6 +489,12 @@ bool TelemetryDeviceDumper::close() localization2DClient.close(); } + if (settings.logIRawValuesPublisher) + { + rawValuesPublisherClient.close(); + } + + bool ok = true; if (settings.saveBufferManagerConfiguration) { auto buffConfToSave = bufferManager.getBufferConfig(); @@ -692,7 +737,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 { @@ -703,12 +748,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; } diff --git a/src/telemetryDeviceDumper/TelemetryDeviceDumper.h b/src/telemetryDeviceDumper/TelemetryDeviceDumper.h index 20f66e9..9992e37 100644 --- a/src/telemetryDeviceDumper/TelemetryDeviceDumper.h +++ b/src/telemetryDeviceDumper/TelemetryDeviceDumper.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -27,6 +28,7 @@ #include #include +#include #include #include #include @@ -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) @@ -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 | * | `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"`. | @@ -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`] + * * @section Example_xml Example of xml * * Example of xml file for using it on the `iCub` robot: @@ -118,6 +125,7 @@ struct TelemetryDeviceDumperSettings { * true * false * true + * false * true * test_telemetry * /home/icub/test_telemetry/ @@ -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}; @@ -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 correctlyConfigured{ false }, sensorsReadCorrectly{false}; std::vector jointPos, jointVel, jointAcc, jointPosErr, jointPosRef, jointTrqErr, jointTrqRef, jointPWM, jointCurr, jointTrq, motorEnc, motorVel, motorAcc, controlModes, interactionModes, odometryData; + + std::map> rawDataValuesMap; + std::vector jointNames; TelemetryDeviceDumperSettings settings; robometry::BufferConfig m_bufferConfig;