iCub-main
Loading...
Searching...
No Matches
FineCalibrationChecker.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2025 iCub Facility - Istituto Italiano di Tecnologia
3 * Author: Jacopo Losi
4 * email: jacopo.losi@iit.it
5 * Permission is granted to copy, distribute, and/or modify this program
6 * under the terms of the GNU General Public License, version 2 or any
7 * later version published by the Free Software Foundation.
8 *
9 * A copy of the license can be found at
10 * http://www.robotcub.org/icub/license/gpl.txt
11 *
12 * This program is distributed in the hope that it will be useful, but
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15 * Public License for more details
16 */
17// -*- mode: C++; tab-width: 4; c-basic-offset: 4; indent-tabs-mode: nil -*-
18
19// std includes
20#include <iostream>
21#include <algorithm>
22#include <unordered_map>
23#include <set>
24#include <utility>
25#include <fstream>
26#include <sstream>
27#include <filesystem>
28#include <chrono>
29
30// yarp includes
31#include <yarp/os/LogStream.h>
32#include <yarp/os/Property.h>
33#include <yarp/os/Log.h>
34#include <yarp/os/Searchable.h>
35
36// APIs
38
39
40namespace
41{
42 YARP_LOG_COMPONENT(FineCalibrationCheckerCOMPONENT, "yarp.device.FineCalibrationChecker")
43}
44
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)
49{
50 // Initialize device driver as empty PolyDriver
51 _remappedControlBoardDevice = std::make_unique<yarp::dev::PolyDriver>();
52 _rawValuesPublisherDevice = std::make_unique<yarp::dev::PolyDriver>();
53}
54
55bool FineCalibrationChecker::open(yarp::os::Searchable& config)
56{
57 // Read configuration file
58 yarp::os::Property property;
59 property.fromString(config.toString().c_str());
60 if (property.isNull())
61 {
62 yCError(FineCalibrationCheckerCOMPONENT) << "Failed to read configuration file. Stopping device...";
63 return false;
64 }
65 else
66 {
67 // Read parameters from the configuration file
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"))
72 {
73 yarp::os::Bottle* _jointsList = property.find("axesNamesList").asList();
74 yarp::os::Bottle &axesNames = _axesNamesList.addList();
75
76 for (size_t i = 0; i < _jointsList->size(); i++)
77 {
78 axesNames.addString(_jointsList->get(i).asString());
79 }
80 }
81 if(property.check("goldPositions"))
82 {
83 yarp::os::Bottle* _goldPositions = property.find("goldPositions").asList();
84 yarp::os::Bottle &goldPositions = _goldPositionsList.addList();
85
86 for (size_t i = 0; i < _goldPositions->size(); i++)
87 {
88 goldPositions.addInt32(_goldPositions->get(i).asInt32());
89 }
90 }
91 if(property.check("calibrationDeltas"))
92 {
93 yarp::os::Bottle* _calibrationDeltas = property.find("calibrationDeltas").asList();
94 yarp::os::Bottle &calibrationDeltas = _calibrationDeltasList.addList();
95
96 for (size_t i = 0; i < _calibrationDeltas->size(); i++)
97 {
98 calibrationDeltas.addFloat64(_calibrationDeltas->get(i).asFloat64());
99 }
100 }
101 if(property.check("encoderResolutions"))
102 {
103 yarp::os::Bottle* _encoderResolutions = property.find("encoderResolutions").asList();
104 yarp::os::Bottle &encoderResolutions = _encoderResolutionsList.addList();
105
106 for (size_t i = 0; i < _encoderResolutions->size(); i++)
107 {
108 encoderResolutions.addInt32(_encoderResolutions->get(i).asInt32());
109 }
110 }
111
112 // Use pointer to list to simplify the listing
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();
117
118 // Check list sizes. They must be equal
119 if (axes->size() != goldpos->size() ||
120 axes->size() != encres->size() ||
121 axes->size() != caldeltas->size())
122 {
123 yCError(FineCalibrationCheckerCOMPONENT) << "Axes names, gold positions and encoder resolutions lists must have the same size. Stopping device...";
124 return false;
125 }
126 else
127 {
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();
132 }
133
134
135 for (size_t i = 0; i < axes->size(); i++)
136 {
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];
142 }
143 }
144
145 _withGui = property.check("withGui", yarp::os::Value(false)).asBool();
146
147 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Initialized device driver";
148 _deviceStatus = deviceStatus::INITIALIZED;
149
150 // Open ports to control boards and network wrapper servers
151 // Open the port to the remote control boards and connect the remappedControlBoardDevice to it
152 yarp::os::Property deviceProperties;
153
154 deviceProperties.put("device", "controlboardremapper");
155 deviceProperties.put("axesNames", _axesNamesList.get(0));
156
157 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Configuring device driver with properties:" << deviceProperties.toString();
158
159 _remappedControlBoardDevice->open(deviceProperties);
160
161 if (!_remappedControlBoardDevice->isValid())
162 {
163 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open device driver. Aborting...";
164 return false;
165 }
166
167 // Open port to the remote raw values publisher server and connect the client network wrapper to it
168 if (_remoteRawValuesPort.empty())
169 {
170 yCError(FineCalibrationCheckerCOMPONENT) << "Remote raw values port is empty. Cannot open device driver. Stopping device...";
171 return false;
172 }
173 else
174 {
175 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Remote raw values port:" << _remoteRawValuesPort;
176
177 }
178
179 yarp::os::Property rawValuesDeviceProperties;
180 rawValuesDeviceProperties.put("device", "rawValuesPublisherClient");
181 rawValuesDeviceProperties.put("remote", _remoteRawValuesPort);
182 rawValuesDeviceProperties.put("local", "/" + _deviceName + "/rawValuesPublisherClient");
183
184 _rawValuesPublisherDevice->open(rawValuesDeviceProperties);
185
186 if (!_rawValuesPublisherDevice->isValid())
187 {
188 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open raw values device driver. Aborting...";
189 return false;
190 }
191
192 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Opened all devices successfully";
193
194 _deviceStatus = deviceStatus::OPENED;
195 return true;
196}
197
199{
200 // Close the device driver and all the opened ports
201 if(_remappedControlBoardDevice->close())
202 {
203 yCDebug(FineCalibrationCheckerCOMPONENT) << "Closed device" << _deviceName;
204 }
205 else
206 {
207 yCError(FineCalibrationCheckerCOMPONENT) << "Unable to close device" << _deviceName;
208 }
209
210 if (_rawValuesPublisherDevice->close())
211 {
212 yCDebug(FineCalibrationCheckerCOMPONENT) << "Closed raw values publisher device";
213 }
214 else
215 {
216 yCError(FineCalibrationCheckerCOMPONENT) << "Unable to close raw values publisher device";
217 }
218 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Closed all devices successfully";
219
220 return true;
221}
222
224{
225 // Initialize the thread --> can add the view on the interfaces here
226 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._imot) || remappedControlBoardInterfaces._imot == nullptr)
227 {
228 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open motor interface. Aborting...";
229 return false;
230 }
231
232 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._ienc) || remappedControlBoardInterfaces._ienc == nullptr)
233 {
234 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open encoders interface. Aborting...";
235 return false;
236 }
237
238
239 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._ienc) || remappedControlBoardInterfaces._ienc == nullptr)
240 {
241 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open encoders interface. Aborting...";
242 return false;
243 }
244
245 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._icontrolcalib) || remappedControlBoardInterfaces._icontrolcalib == nullptr)
246 {
247 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open control calibration interface. Aborting...";
248 return false;
249 }
250
251 if (!_rawValuesPublisherDevice->view(_iravap) || _iravap == nullptr)
252 {
253 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open raw values publisher interface. Aborting...";
254 return false;
255 }
256
257
258 // Configuring raw values metadata
259 rawDataMetadata = {};
260 _iravap->getMetadataMap(rawDataMetadata);
261 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Configured raw values with metadata";
262 for (auto [k, m] : rawDataMetadata.metadataMap)
263 {
264 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "\n"
265 << "\t Key: " << k << "\n"
266 << "\t axesName: " << m.axesNames << "\n"
267 << "\t rawValueNames: " << m.rawValueNames
268 ;
269 }
270
271 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Opened remote calibrator and control calibration interfaces successfully";
272
273
274 _deviceStatus = deviceStatus::CONFIGURED;
275 return true;
276
277}
278
280{
281 // Run the calibration thread
282 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Thread started to run";
283
284 // Use chrono for time-based debug prints
285 static auto lastTimerLog = std::chrono::steady_clock::now();
286 static auto shoutdownTimer = std::chrono::steady_clock::now();
287
288 while(!this->isStopping())
289 {
290 if (_deviceStatus == deviceStatus::CONFIGURED)
291 {
292 auto now = std::chrono::steady_clock::now();
293 if (std::chrono::duration_cast<std::chrono::milliseconds>(now - lastTimerLog).count() > 1000)
294 {
295 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Device configured, waiting for attachAll() to be called.";
296 lastTimerLog = now;
297 }
298 yarp::os::Time::delay(0.1); // Prevent busy-waiting.
299 continue;
300 }
301 else if(_deviceStatus == deviceStatus::STARTED)
302 {
303 // Get number of axes
304 int numAxes = 0;
305 if(!_axesNamesList.isNull() && _axesNamesList.size() > 0 && _axesNamesList.get(0).isList())
306 {
307 numAxes = _axesNamesList.get(0).asList()->size();
308 }
309 // Check if calibration is done by calling IControlCalibration APIs
310 bool calibDone = true;
311 for (size_t i = 0; i < numAxes; i++)
312 {
313 //calibDone &= remappedControlBoardInterfaces._icontrolcalib->calibrationDone(i);
314 // TODO: add list of axis that did not perform calibration to be rechecked
315 }
316 if (!calibDone)
317 {
318 yCWarning(FineCalibrationCheckerCOMPONENT) << _deviceName << "Calib not complete for all axis. Waiting for calibration to finish...";
319 }
320 else
321 {
322 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Calibration done successfully";
323 _deviceStatus = deviceStatus::CALIBRATED;
324 }
325 }
326 else if(_deviceStatus == deviceStatus::CALIBRATED)
327 {
328 // TODO: Check that all joints reached the startup/zero position
329 _deviceStatus = deviceStatus::IN_ZERO_POSITION;
330 }
331 else if(_deviceStatus == deviceStatus::IN_ZERO_POSITION)
332 {
333 if(!_iravap->getRawDataMap(rawDataValuesMap))
334 {
335 yCWarning(FineCalibrationCheckerCOMPONENT) << "embObjMotionControl::IRawValuesPublisher warning : raw_data_values map was not read correctly";
336 }
337 else
338 {
339 yCDebug(FineCalibrationCheckerCOMPONENT) << "Get raw values from encoders:";
340 for (auto [key,value] : rawDataValuesMap)
341 {
342 yCDebug(FineCalibrationCheckerCOMPONENT) << "\t key:" << key << "value:" << value;
343 }
344
345 // Here we have to evaluate the delta between the raw golen position
346 // and the raw position read at the hard stop per each axis
347 // TODO: input file not needed anymore. RobeRemoved
348 evaluateHardStopPositionDelta(_rawValuesTag, "zeroPositionsDataDelta.csv");
349 _deviceStatus = deviceStatus::CHECK_COMPLETED;
350 }
351 }
352 else if(_deviceStatus == deviceStatus::CHECK_COMPLETED)
353 {
354 auto now = std::chrono::steady_clock::now();
355 if (std::chrono::duration_cast<std::chrono::milliseconds>(now - shoutdownTimer).count() > 5000)
356 {
357 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Operation completed successfully. Waiting yarprobotinterface to stop the thread...";
358 // shoutdownTimer = now;
359 break; // Exit the loop to stop the thread
360 }
361 }
362 else
363 {
364 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Device is in unknown state. Stopping thread...";
365 break; // Exit the loop to stop the thread
366
367 }
368 }
369
370 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Thread stopping";
371 // Perform any cleanup or finalization tasks here
372 // For example, you can close the device or release resources
373
374}
375
377{
378 //nothing to do for now
379}
380
381bool FineCalibrationChecker::attachAll(const yarp::dev::PolyDriverList& device2attach)
382{
383 // Attach all devices to the FineCalibrationChecker
384 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Attaching all devices";
385
386 // The _imultwrap view should be place before calling the attachToAllControlBoards method
387 // otherwise IMultipleWrapper::attachAll cannot be used
388 if (!_remappedControlBoardDevice->view(remappedControlBoardInterfaces._imultwrap) || remappedControlBoardInterfaces._imultwrap == nullptr)
389 {
390 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Unable to open multiple wrapper interface. Aborting...";
391 return false;
392 }
393
394 if (!_remappedControlBoardDevice->isValid())
395 {
396 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Device is not valid. Cannot attach.";
397 return false;
398 }
399
400 // Attach the device to the thread
401 if (!this->attachToAllControlBoards(device2attach))
402 {
403 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Failed to attach all devices.";
404 return false;
405 }
406
407 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Successfully attached all devices. Starting the thread...";
408 // Start the thread
409 if (!this->start())
410 {
411 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Failed to start the thread.";
412 return false;
413 }
414 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Thread started successfully";
415 _deviceStatus = deviceStatus::STARTED;
416
417 return true;
418}
419
421{
422 // Stop the thread and detach all devices
423 if (this->isRunning())
424 {
425 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Stopping the thread before detaching all devices";
426 this->stop();
427 }
428 if(!remappedControlBoardInterfaces._imultwrap->detachAll())
429 {
430 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Failed to detach all devices";
431 return false;
432 }
433 else
434 {
435 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Detached all devices successfully";
436 }
437 return true;
438}
439
440// Private methods
441
443{
444 // Check if the calibration was successful
445 return (_deviceStatus == deviceStatus::CALIBRATED);
446}
447
448bool FineCalibrationChecker::attachToAllControlBoards(const yarp::dev::PolyDriverList& polyList)
449{
450 // Attach all control boards to the FineCalibrationChecker
451 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Attaching all control boards";
452 if (!_remappedControlBoardDevice->isValid())
453 {
454 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Device is not valid. Cannot attach.";
455 return false;
456 }
457
458 // Setup the control board list given the list of PolyDrivers devices we need to attach to.
459 yarp::dev::PolyDriverList controlBoardsList;
460 for (size_t i = 0; i < polyList.size(); i++)
461 {
462 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Attaching control board" << polyList[i]->key;
463 controlBoardsList.push(const_cast<yarp::dev::PolyDriverDescriptor&>(*polyList[i]));
464 }
465
466 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Control boards list size:" << controlBoardsList.size();
467
468 // Attach the list of control boards to the control board remapper
469 if(!remappedControlBoardInterfaces._imultwrap->attachAll(controlBoardsList))
470 {
471 yCError(FineCalibrationCheckerCOMPONENT) << _deviceName << "Failed to attach all control boards";
472 return false;
473 }
474
475 yCDebug(FineCalibrationCheckerCOMPONENT) << _deviceName << "Successfully attached all control boards";
476
477 return true;
478}
479
480void FineCalibrationChecker::evaluateHardStopPositionDelta(const std::string& key, const std::string& outputFileName)
481{
482
483 // Add comments for magic numbers
484 const int32_t RAW_VALUES_STRIDE = 3; // Number of raw values per joint in the raw data vector
485 const int64_t ICUB_DEGREES_RANGE = (65535); // Range of iCub degrees for rescaling (2^16-1)
486 // Get the directory of the output file
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;
492 int64_t delta = 0;
493 std::vector<ItemData> sampleItems = {};
494
495 yarp::os::Bottle* caldeltas = _calibrationDeltasList.get(0).asList();
496
497 std::vector<int64_t> homePositions = {30, 30, 0, 50}; // Assuming home positions are all zero for simplicity
498 std::vector<double> calibrationDelta = {0, -9.2, -17.1, -2.7}; // Assuming raw home positions are all zero for simplicity
499 std::ofstream outFile(outputPath);
500 if (!outFile.is_open())
501 {
502 yCError(FineCalibrationCheckerCOMPONENT) << "Unable to open output file:" << outputPath.string();
503 return;
504 }
505 // Add headers to the output CSV file
506 outFile << "AxisName,GoldPosition,RescaledPosition,RawPosition,Delta\n";
507 yCDebug(FineCalibrationCheckerCOMPONENT) << "Evaluating deltas.....";
508 if(auto it = rawDataValuesMap.find(key); it != rawDataValuesMap.end())
509 {
510 std::vector<std::string> axesNames = {};
511 std::vector<std::int32_t> rawData = {};
512 _iravap->getAxesNames(it->first, axesNames);
513 _iravap->getRawData(it->first, rawData);
514
515 for (size_t i = 0; i < axesNames.size(); ++i)
516 {
517 goldPosition = 0;
518 rawPosition = 0;
519 resolution = 0;
520 delta = 0;
521 yCDebug(FineCalibrationCheckerCOMPONENT) << "Evaluating axis:" << axesNames[i];
522 if (auto it = axesRawGoldenPositionsResMap.find(axesNames[i]); it != axesRawGoldenPositionsResMap.end())
523 {
524 calibrationDelta[i] = caldeltas->get(i).asFloat64(); // Get the calibration delta for the axis
525 double pos = 0.0;
526 remappedControlBoardInterfaces._ienc->getEncoder(i, &pos); // Update home position by calling the IEncoders API
527 homePositions[i] = (pos > 0) ? static_cast<int64_t>(pos) : static_cast<int64_t>(-pos); // Update home position for the axis
528 goldPosition = it->second[0];
529 resolution = it->second[1];
530 rawPosition = rawData[RAW_VALUES_STRIDE*i]; // This because the raw values for tag eoprot_tag_mc_joint_status_addinfo_multienc
531 // are stored in a vector whose legth is joints_number*3, where each sub-array is made such
532 // [raw_val_primary_enc, raw_val_secondary_enc, rraw_val_auxiliary_enc]
533 // and we want the first value for each joint
534 rescaledPos = rawPosition * ICUB_DEGREES_RANGE / resolution; // Rescale the encoder raw position to iCubDegrees
535 delta = std::abs(goldPosition - rescaledPos) / (ICUB_DEGREES_RANGE/360) - homePositions[i]; // Calculate the delta in degrees
536 delta = std::abs(delta) + static_cast<int64_t>(calibrationDelta[i]); // Add the calibration delta to the delta
537 yCDebug(FineCalibrationCheckerCOMPONENT) << "GP:" << goldPosition << "HP:" << homePositions[i] << "RSP:" << rescaledPos << "RWP:" << rawPosition << "DD:" << delta;
538 }
539 else
540 {
541 yCWarning(FineCalibrationCheckerCOMPONENT) << "This device axes has not ben requested to be checked. Continue...";
542 continue;
543 }
544
545 // Write to output CSV file
546 outFile << axesNames[i] << "," << goldPosition << "," << rescaledPos << "," << rawPosition << "," << delta << "\n";
547 sampleItems.push_back({axesNames[i], goldPosition, rescaledPos, rawPosition, delta});
548 }
549 }
550 else
551 {
552 yCError(FineCalibrationCheckerCOMPONENT) << "Key" << key << "not found in rawDataValuesMap";
553 outFile << "Key not found in rawDataValuesMap\n";
554 }
555
556 outFile.close();
557 yCDebug(FineCalibrationCheckerCOMPONENT) << "Output CSV written to:" << outputPath.string();
558 // Generate output image
559 if(_withGui) {
560 generateOutputImage(1800, 400, sampleItems);
561 }
562}
563
564void FineCalibrationChecker::generateOutputImage(int frameWidth, int frameHeight, const std::vector<ItemData>& items)
565{
566 cv::Mat image = cv::Mat::zeros(frameHeight, frameWidth, CV_8UC3);
567 image.setTo(cv::Scalar(255, 255, 255)); // White background
568
569 int lineHeight = 30;
570 int padding = 10;
571 int textOffset = 20;
572 int colWidthAvg = (frameWidth - 2 * padding) / 5; // Average width for each column
573
574 // Define headers and column widths (in pixels)
575 std::vector<std::string> headers = {"AxisName", "GoldPosition[iCubDegrees]", "RescaledPosition[iCubDegrees]", "RawPosition[Ticks]", "Delta[Degrees]"};
576 std::vector<int> colWidths(5, colWidthAvg); // Creates a vector of size 5, all values set to 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];
581 }
582
583 // Draw header row
584 int headerY = padding;
585 cv::Scalar headerBgColor(220, 220, 220); // Light gray background
586 // Draw the filled background for the header row (light gray)
587 cv::rectangle(image,
588 cv::Point(padding, headerY),
589 cv::Point(frameWidth - padding, headerY + lineHeight - 5),
590 headerBgColor, cv::FILLED);
591 // Draw a border around the header row (darker gray)
592 cv::rectangle(image,
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);
599 }
600
601 // Draw data rows
602 for (size_t i = 0; i < items.size(); ++i)
603 {
604 int y = padding + (i + 1) * lineHeight; // +1 to account for header
605 // Draw background rectangle for the row
606 cv::Scalar bgColor = getColorForDelta(items[i].val4, 1, 3);
607 cv::rectangle(image,
608 cv::Point(padding, y),
609 cv::Point(frameWidth - padding, y + lineHeight - 5),
610 bgColor, cv::FILLED);
611
612 // Draw border for the row
613 cv::rectangle(image,
614 cv::Point(padding, y),
615 cv::Point(frameWidth - padding, y + lineHeight - 5),
616 cv::Scalar(200, 200, 200), 1);
617
618 // Draw each column value aligned with header
619 std::vector<std::string> rowVals = {
620 items[i].name,
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)
625 };
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);
629 }
630 }
631
632 cv::imwrite("output_frame.png", image);
633 //TODO: remove openCV highgui functions since they are not thread safe and showld be called only in the main thread
634 cv::imshow("Output Frame", image);
635 cv::waitKey(0);
636 cv::destroyAllWindows();
637}
638
639cv::Scalar FineCalibrationChecker::getColorForDelta(int32_t delta, int32_t threshold_1, int32_t threshold_2)
640{
641 if (std::abs(delta) > threshold_2) return cv::Scalar(0, 0, 255); // Red
642 else if (std::abs(delta) > threshold_1) return cv::Scalar(0, 165, 255); // Orange
643 else return cv::Scalar(0, 255, 0); // Green
644}
645
bool open(yarp::os::Searchable &config) override
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
std::map< std::string, rawValuesKeyMetadata > metadataMap
FILE * outFile
Copyright (C) 2008 RobotCub Consortium.