22#include <unordered_map>
31#include <yarp/os/LogStream.h>
32#include <yarp/os/Property.h>
33#include <yarp/os/Log.h>
34#include <yarp/os/Searchable.h>
42 YARP_LOG_COMPONENT(FineCalibrationCheckerCOMPONENT,
"yarp.device.FineCalibrationChecker")
46 :
yarp::os::Thread(), _deviceName(
"fineCalibrationChecker"), _portPrefix(
"/fineCalibrationChecker"),
47 _robotName(
"icub"), _remoteRawValuesPort(
"/icub/rawvalues"), _axesNamesList(
yarp::os::Bottle()),
48 _goldPositionsList(
yarp::os::Bottle()), _encoderResolutionsList(
yarp::os::Bottle()), _calibrationDeltasList(
yarp::os::Bottle()), _deviceStatus(
deviceStatus::NONE)
51 _remappedControlBoardDevice = std::make_unique<yarp::dev::PolyDriver>();
52 _rawValuesPublisherDevice = std::make_unique<yarp::dev::PolyDriver>();
58 yarp::os::Property property;
59 property.fromString(config.toString().c_str());
60 if (property.isNull())
62 yCError(FineCalibrationCheckerCOMPONENT) <<
"Failed to read configuration file. Stopping device...";
68 if (property.check(
"devicename")) { _deviceName =
property.find(
"devicename").asString(); }
69 if (property.check(
"robotname")) { _robotName =
property.find(
"robotname").asString(); }
70 if (property.check(
"remoteRawValuesPort")) { _remoteRawValuesPort =
property.find(
"remoteRawValuesPort").asString(); }
71 if (property.check(
"axesNamesList"))
73 yarp::os::Bottle* _jointsList =
property.find(
"axesNamesList").asList();
74 yarp::os::Bottle &axesNames = _axesNamesList.addList();
76 for (
size_t i = 0; i < _jointsList->size(); i++)
78 axesNames.addString(_jointsList->get(i).asString());
81 if(property.check(
"goldPositions"))
83 yarp::os::Bottle* _goldPositions =
property.find(
"goldPositions").asList();
84 yarp::os::Bottle &goldPositions = _goldPositionsList.addList();
86 for (
size_t i = 0; i < _goldPositions->size(); i++)
88 goldPositions.addInt32(_goldPositions->get(i).asInt32());
91 if(property.check(
"calibrationDeltas"))
93 yarp::os::Bottle* _calibrationDeltas =
property.find(
"calibrationDeltas").asList();
94 yarp::os::Bottle &calibrationDeltas = _calibrationDeltasList.addList();
96 for (
size_t i = 0; i < _calibrationDeltas->size(); i++)
98 calibrationDeltas.addFloat64(_calibrationDeltas->get(i).asFloat64());
101 if(property.check(
"encoderResolutions"))
103 yarp::os::Bottle* _encoderResolutions =
property.find(
"encoderResolutions").asList();
104 yarp::os::Bottle &encoderResolutions = _encoderResolutionsList.addList();
106 for (
size_t i = 0; i < _encoderResolutions->size(); i++)
108 encoderResolutions.addInt32(_encoderResolutions->get(i).asInt32());
113 yarp::os::Bottle* axes = _axesNamesList.get(0).asList();
114 yarp::os::Bottle* goldpos = _goldPositionsList.get(0).asList();
115 yarp::os::Bottle* encres = _encoderResolutionsList.get(0).asList();
116 yarp::os::Bottle* caldeltas = _calibrationDeltasList.get(0).asList();
119 if (axes->size() != goldpos->size() ||
120 axes->size() != encres->size() ||
121 axes->size() != caldeltas->size())
123 yCError(FineCalibrationCheckerCOMPONENT) <<
"Axes names, gold positions and encoder resolutions lists must have the same size. Stopping device...";
128 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Axes names list:" << _axesNamesList.toString();
129 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Gold positions list:" << goldpos->toString();
130 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Encoder resolutions list:" << encres->toString();
131 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Calibration deltas list:" << caldeltas->toString();
135 for (
size_t i = 0; i < axes->size(); i++)
137 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Adding to MAP key:" << axes->get(i).asString()
138 <<
"GP:" << goldpos->get(i).asInt32() <<
"ER:" << encres->get(i).asInt32() <<
"CD:" << caldeltas->get(i).asFloat64();
139 axesRawGoldenPositionsResMap[axes->get(i).asString()] = {goldpos->get(i).asInt32(), encres->get(i).asInt32()};
140 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Array added to MAP:" << axesRawGoldenPositionsResMap.at(axes->get(i).asString())[0]
141 <<
"and" << axesRawGoldenPositionsResMap.at(axes->get(i).asString())[1];
145 _withGui =
property.check(
"withGui", yarp::os::Value(
false)).asBool();
147 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Initialized device driver";
152 yarp::os::Property deviceProperties;
154 deviceProperties.put(
"device",
"controlboardremapper");
155 deviceProperties.put(
"axesNames", _axesNamesList.get(0));
157 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Configuring device driver with properties:" << deviceProperties.toString();
159 _remappedControlBoardDevice->open(deviceProperties);
161 if (!_remappedControlBoardDevice->isValid())
163 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open device driver. Aborting...";
168 if (_remoteRawValuesPort.empty())
170 yCError(FineCalibrationCheckerCOMPONENT) <<
"Remote raw values port is empty. Cannot open device driver. Stopping device...";
175 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Remote raw values port:" << _remoteRawValuesPort;
179 yarp::os::Property rawValuesDeviceProperties;
180 rawValuesDeviceProperties.put(
"device",
"rawValuesPublisherClient");
181 rawValuesDeviceProperties.put(
"remote", _remoteRawValuesPort);
182 rawValuesDeviceProperties.put(
"local",
"/" + _deviceName +
"/rawValuesPublisherClient");
184 _rawValuesPublisherDevice->open(rawValuesDeviceProperties);
186 if (!_rawValuesPublisherDevice->isValid())
188 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open raw values device driver. Aborting...";
192 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Opened all devices successfully";
201 if(_remappedControlBoardDevice->close())
203 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Closed device" << _deviceName;
207 yCError(FineCalibrationCheckerCOMPONENT) <<
"Unable to close device" << _deviceName;
210 if (_rawValuesPublisherDevice->close())
212 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Closed raw values publisher device";
216 yCError(FineCalibrationCheckerCOMPONENT) <<
"Unable to close raw values publisher device";
218 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Closed all devices successfully";
226 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._imot) || remappedControlBoardInterfaces._imot ==
nullptr)
228 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open motor interface. Aborting...";
232 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._ienc) || remappedControlBoardInterfaces._ienc ==
nullptr)
234 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open encoders interface. Aborting...";
239 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._ienc) || remappedControlBoardInterfaces._ienc ==
nullptr)
241 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open encoders interface. Aborting...";
245 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._icontrolcalib) || remappedControlBoardInterfaces._icontrolcalib ==
nullptr)
247 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open control calibration interface. Aborting...";
251 if (!_rawValuesPublisherDevice->view(_iravap) || _iravap ==
nullptr)
253 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open raw values publisher interface. Aborting...";
259 rawDataMetadata = {};
261 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Configured raw values with metadata";
264 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"\n"
265 <<
"\t Key: " << k <<
"\n"
266 <<
"\t axesName: " << m.axesNames <<
"\n"
267 <<
"\t rawValueNames: " << m.rawValueNames
271 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Opened remote calibrator and control calibration interfaces successfully";
282 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Thread started to run";
285 static auto lastTimerLog = std::chrono::steady_clock::now();
286 static auto shoutdownTimer = std::chrono::steady_clock::now();
288 while(!this->isStopping())
292 auto now = std::chrono::steady_clock::now();
293 if (std::chrono::duration_cast<std::chrono::milliseconds>(now - lastTimerLog).count() > 1000)
295 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Device configured, waiting for attachAll() to be called.";
298 yarp::os::Time::delay(0.1);
305 if(!_axesNamesList.isNull() && _axesNamesList.size() > 0 && _axesNamesList.get(0).isList())
307 numAxes = _axesNamesList.get(0).asList()->size();
310 bool calibDone =
true;
311 for (
size_t i = 0; i < numAxes; i++)
318 yCWarning(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Calib not complete for all axis. Waiting for calibration to finish...";
322 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Calibration done successfully";
335 yCWarning(FineCalibrationCheckerCOMPONENT) <<
"embObjMotionControl::IRawValuesPublisher warning : raw_data_values map was not read correctly";
339 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Get raw values from encoders:";
340 for (
auto [key,value] : rawDataValuesMap)
342 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"\t key:" << key <<
"value:" << value;
348 evaluateHardStopPositionDelta(_rawValuesTag,
"zeroPositionsDataDelta.csv");
354 auto now = std::chrono::steady_clock::now();
355 if (std::chrono::duration_cast<std::chrono::milliseconds>(now - shoutdownTimer).count() > 5000)
357 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Operation completed successfully. Waiting yarprobotinterface to stop the thread...";
364 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Device is in unknown state. Stopping thread...";
370 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Thread stopping";
384 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Attaching all devices";
388 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._imultwrap) || remappedControlBoardInterfaces._imultwrap ==
nullptr)
390 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Unable to open multiple wrapper interface. Aborting...";
394 if (!_remappedControlBoardDevice->isValid())
396 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Device is not valid. Cannot attach.";
401 if (!this->attachToAllControlBoards(device2attach))
403 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Failed to attach all devices.";
407 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Successfully attached all devices. Starting the thread...";
411 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Failed to start the thread.";
414 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Thread started successfully";
423 if (this->isRunning())
425 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Stopping the thread before detaching all devices";
428 if(!remappedControlBoardInterfaces._imultwrap->detachAll())
430 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Failed to detach all devices";
435 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Detached all devices successfully";
448bool FineCalibrationChecker::attachToAllControlBoards(
const yarp::dev::PolyDriverList& polyList)
451 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Attaching all control boards";
452 if (!_remappedControlBoardDevice->isValid())
454 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Device is not valid. Cannot attach.";
459 yarp::dev::PolyDriverList controlBoardsList;
460 for (
size_t i = 0; i < polyList.size(); i++)
462 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Attaching control board" << polyList[i]->key;
463 controlBoardsList.push(
const_cast<yarp::dev::PolyDriverDescriptor&
>(*polyList[i]));
466 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Control boards list size:" << controlBoardsList.size();
469 if(!remappedControlBoardInterfaces._imultwrap->attachAll(controlBoardsList))
471 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Failed to attach all control boards";
475 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName <<
"Successfully attached all control boards";
480void FineCalibrationChecker::evaluateHardStopPositionDelta(
const std::string& key,
const std::string& outputFileName)
484 const int32_t RAW_VALUES_STRIDE = 3;
485 const int64_t ICUB_DEGREES_RANGE = (65535);
487 std::filesystem::path outputPath(outputFileName);
488 int64_t goldPosition = 0;
489 int64_t rawPosition = 0;
490 int64_t resolution = 0;
491 int64_t rescaledPos = 0;
493 std::vector<ItemData> sampleItems = {};
495 yarp::os::Bottle* caldeltas = _calibrationDeltasList.get(0).asList();
497 std::vector<int64_t> homePositions = {30, 30, 0, 50};
498 std::vector<double> calibrationDelta = {0, -9.2, -17.1, -2.7};
499 std::ofstream
outFile(outputPath);
502 yCError(FineCalibrationCheckerCOMPONENT) <<
"Unable to open output file:" << outputPath.string();
506 outFile <<
"AxisName,GoldPosition,RescaledPosition,RawPosition,Delta\n";
507 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Evaluating deltas.....";
508 if(
auto it = rawDataValuesMap.find(key); it != rawDataValuesMap.end())
510 std::vector<std::string> axesNames = {};
511 std::vector<std::int32_t> rawData = {};
515 for (
size_t i = 0; i < axesNames.size(); ++i)
521 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Evaluating axis:" << axesNames[i];
522 if (
auto it = axesRawGoldenPositionsResMap.find(axesNames[i]); it != axesRawGoldenPositionsResMap.end())
524 calibrationDelta[i] = caldeltas->get(i).asFloat64();
526 remappedControlBoardInterfaces._ienc->getEncoder(i, &pos);
527 homePositions[i] = (pos > 0) ?
static_cast<int64_t
>(pos) :
static_cast<int64_t
>(-pos);
528 goldPosition = it->second[0];
529 resolution = it->second[1];
530 rawPosition = rawData[RAW_VALUES_STRIDE*i];
534 rescaledPos = rawPosition * ICUB_DEGREES_RANGE / resolution;
535 delta = std::abs(goldPosition - rescaledPos) / (ICUB_DEGREES_RANGE/360) - homePositions[i];
536 delta = std::abs(delta) +
static_cast<int64_t
>(calibrationDelta[i]);
537 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"GP:" << goldPosition <<
"HP:" << homePositions[i] <<
"RSP:" << rescaledPos <<
"RWP:" << rawPosition <<
"DD:" << delta;
541 yCWarning(FineCalibrationCheckerCOMPONENT) <<
"This device axes has not ben requested to be checked. Continue...";
546 outFile << axesNames[i] <<
"," << goldPosition <<
"," << rescaledPos <<
"," << rawPosition <<
"," << delta <<
"\n";
547 sampleItems.push_back({axesNames[i], goldPosition, rescaledPos, rawPosition, delta});
552 yCError(FineCalibrationCheckerCOMPONENT) <<
"Key" << key <<
"not found in rawDataValuesMap";
553 outFile <<
"Key not found in rawDataValuesMap\n";
557 yCDebug(FineCalibrationCheckerCOMPONENT) <<
"Output CSV written to:" << outputPath.string();
560 generateOutputImage(1800, 400, sampleItems);
564void FineCalibrationChecker::generateOutputImage(
int frameWidth,
int frameHeight,
const std::vector<ItemData>& items)
566 cv::Mat image = cv::Mat::zeros(frameHeight, frameWidth, CV_8UC3);
567 image.setTo(cv::Scalar(255, 255, 255));
572 int colWidthAvg = (frameWidth - 2 * padding) / 5;
575 std::vector<std::string> headers = {
"AxisName",
"GoldPosition[iCubDegrees]",
"RescaledPosition[iCubDegrees]",
"RawPosition[Ticks]",
"Delta[Degrees]"};
576 std::vector<int> colWidths(5, colWidthAvg);
577 std::vector<int> colX(headers.size());
578 colX[0] = padding + 5;
579 for (
size_t i = 1; i < headers.size(); ++i) {
580 colX[i] = colX[i-1] + colWidths[i-1];
584 int headerY = padding;
585 cv::Scalar headerBgColor(220, 220, 220);
588 cv::Point(padding, headerY),
589 cv::Point(frameWidth - padding, headerY + lineHeight - 5),
590 headerBgColor, cv::FILLED);
593 cv::Point(padding, headerY),
594 cv::Point(frameWidth - padding, headerY + lineHeight - 5),
595 cv::Scalar(100, 100, 100), 1);
596 for (
size_t c = 0; c < headers.size(); ++c) {
597 cv::putText(image, headers[c], cv::Point(colX[c], headerY + textOffset),
598 cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 0), 2);
602 for (
size_t i = 0; i < items.size(); ++i)
604 int y = padding + (i + 1) * lineHeight;
606 cv::Scalar bgColor = getColorForDelta(items[i].val4, 1, 3);
608 cv::Point(padding,
y),
609 cv::Point(frameWidth - padding,
y + lineHeight - 5),
610 bgColor, cv::FILLED);
614 cv::Point(padding,
y),
615 cv::Point(frameWidth - padding,
y + lineHeight - 5),
616 cv::Scalar(200, 200, 200), 1);
619 std::vector<std::string> rowVals = {
621 std::to_string(items[i].val1),
622 std::to_string(items[i].val2),
623 std::to_string(items[i].val3),
624 std::to_string(items[i].val4)
626 for (
size_t c = 0; c < rowVals.size(); ++c) {
627 cv::putText(image, rowVals[c], cv::Point(colX[c],
y + textOffset),
628 cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 0), 2);
632 cv::imwrite(
"output_frame.png", image);
634 cv::imshow(
"Output Frame", image);
636 cv::destroyAllWindows();
639cv::Scalar FineCalibrationChecker::getColorForDelta(int32_t delta, int32_t threshold_1, int32_t threshold_2)
641 if (std::abs(delta) > threshold_2)
return cv::Scalar(0, 0, 255);
642 else if (std::abs(delta) > threshold_1)
return cv::Scalar(0, 165, 255);
643 else return cv::Scalar(0, 255, 0);
bool threadInit() override
bool open(yarp::os::Searchable &config) override
bool detachAll() override
bool isCalibrationSuccessful() const
bool attachAll(const yarp::dev::PolyDriverList &device2attach) override
virtual bool getMetadataMap(rawValuesKeyMetadataMap &metamap)=0
virtual bool getRawData(std::string key, std::vector< std::int32_t > &data)=0
virtual bool getAxesNames(std::string key, std::vector< std::string > &axesNames)=0
virtual bool getRawDataMap(std::map< std::string, std::vector< std::int32_t > > &map)=0
Copyright (C) 2008 RobotCub Consortium.