Skip to content

Commit

Permalink
Update telemetry for rawValuesPublisher
Browse files Browse the repository at this point in the history 
Update CMakeLists
Update telemetrycode adding call to metadata
  • Loading branch information
@MSECode
MSECode committed Oct 4, 2024
1 parent bb2ea36 commit 3143e6c
Show file tree
Hide file tree
Showing 3 changed files with 90 additions and 7 deletions.
6 changes: 4 additions & 2 deletions src/telemetryDeviceDumper/CMakeLists.txt
View file
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)
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
73 changes: 70 additions & 3 deletions src/telemetryDeviceDumper/TelemetryDeviceDumper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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()) {
Expand Down Expand Up @@ -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) {
Expand All @@ -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)
{
Expand Down Expand Up @@ -363,6 +387,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 +437,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 +447,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 +487,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 +735,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 +746,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
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 |
* | `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`]
*
* @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

0 comments on commit 3143e6c

Please sign in to comment.