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
Fix the use of logControlBoardQuantities flag
  • Loading branch information
@MSECode
MSECode committed Oct 9, 2024
1 parent bb2ea36 commit bee1486
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 14 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
89 changes: 79 additions & 10 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 @@ -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;
Expand Down Expand Up @@ -363,6 +389,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 +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;
}
Expand All @@ -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<uint16_t>(m.size), 1}, m.rawValueNames });
}
}

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

if (ok)
{
correctlyConfigured = true;
Expand Down Expand Up @@ -450,6 +489,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 +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
{
Expand All @@ -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;
}
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 bee1486

Please sign in to comment.