iCub-main
Loading...
Searching...
No Matches
FineCalibrationChecker.h
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#ifndef FINE_CALIBRATION_CHECKER_THREAD_H
20#define FINE_CALIBRATION_CHECKER_THREAD_H
21
22// std includes
23#include <vector>
24#include <string>
25#include <map>
26#include <memory>
27
28#include <opencv2/opencv.hpp>
29
30// yarp includes
31#include <yarp/os/Bottle.h>
32#include <yarp/os/Thread.h>
33#include <yarp/dev/PolyDriver.h>
34#include <yarp/dev/DeviceDriver.h>
35#include <yarp/dev/IMultipleWrapper.h>
36#include <yarp/dev/IControlCalibration.h>
37#include <yarp/dev/IMotor.h>
38#include <yarp/dev/IEncoders.h>
39
40// iCub includes
42
43
44struct ItemData {
45 std::string name;
46 int64_t val1, val2, val3, val4;
47};
48
49class FineCalibrationChecker : public yarp::dev::DeviceDriver,
50 public yarp::os::Thread,
51 public yarp::dev::IMultipleWrapper
52{
53public:
54 // Enum for device status
55 enum class deviceStatus
56 {
57 INITIALIZED = 0,
58 OPENED = 1,
59 CONFIGURED = 2,
60 STARTED = 3,
61 CALIBRATED = 4,
64 UNKNOWN = 244,
65 NONE = 255
66 };
67
68 // Constructor
70
71 // Destructor
73
74 // Copy constructor
76
77 // Copy assignment operator
79
80 // Move constructor
82
83 // Move assignment operator
85
86 // Overridden methods from yarp::dev::DeviceDriver
87 bool open(yarp::os::Searchable& config) override;
88 bool close() override;
89
90 // Overridden methods from yarp::os::Thread
91 void run() override;
92 void onStop() override;
93 bool threadInit() override;
94
95 // Overridden methods from yarp::dev::IMultipleWrapper
96 bool attachAll(const yarp::dev::PolyDriverList& device2attach) override;
97 bool detachAll() override;
98
99 bool isCalibrationSuccessful() const;
100
101private:
102 // Private members
103
104 // Configuration parameters
105 std::string _portPrefix = "/fineCalibrationChecker";
106 std::string _robotName= "";
107 std::string _deviceName= "fineCalibrationChecker";
108 std::string _remoteRawValuesPort = "";
109 bool _withGui = false;
110
111 deviceStatus _deviceStatus = deviceStatus::NONE;
112
113 yarp::os::Bottle _axesNamesList = yarp::os::Bottle();
114 yarp::os::Bottle _goldPositionsList = yarp::os::Bottle();
115 yarp::os::Bottle _encoderResolutionsList = yarp::os::Bottle();
116 yarp::os::Bottle _calibrationDeltasList = yarp::os::Bottle();
117
118 std::vector<std::string> _robotSubpartsWrapper = {"setup_mc", "head", "left_arm", "right_arm", "torso", "left_leg", "right_leg"};
119 std::map<std::string, std::vector<std::int32_t>> rawDataValuesMap;
120 iCub::rawValuesKeyMetadataMap rawDataMetadata;
121 std::map<std::string, std::array<int32_t, 2>> axesRawGoldenPositionsResMap;
122 std::string _rawValuesTag = "eoprot_tag_mc_joint_status_addinfo_multienc";
123
124 // Pointers to interfaces
125 struct
126 {
127 yarp::dev::IMultipleWrapper* _imultwrap{ nullptr };
128 yarp::dev::IControlCalibration* _icontrolcalib { nullptr };
129 yarp::dev::IMotor* _imot { nullptr };
130 yarp::dev::IEncoders* _ienc { nullptr };
131 } remappedControlBoardInterfaces;
132
134
135 // Client drivers to communicate with interfaces
136 std::unique_ptr<yarp::dev::PolyDriver> _remappedControlBoardDevice;
137 std::unique_ptr<yarp::dev::PolyDriver> _rawValuesPublisherDevice;
138
139 void evaluateHardStopPositionDelta(const std::string& key, const std::string& outputFileName);
140 void generateOutputImage(int frameWidth, int frameHeight, const std::vector<ItemData>& items);
141
142 // Utility methods
143 cv::Scalar getColorForDelta(int32_t delta, int32_t threshold_1, int32_t threshold_2);
144
145 bool attachToAllControlBoards(const yarp::dev::PolyDriverList& polyList);
146};
147
148#endif // FINE_CALIBRATION_CHECKER_THREAD_H
FineCalibrationChecker(const FineCalibrationChecker &other)=default
bool open(yarp::os::Searchable &config) override
FineCalibrationChecker & operator=(const FineCalibrationChecker &other)=default
FineCalibrationChecker(FineCalibrationChecker &&other) noexcept=default
bool attachAll(const yarp::dev::PolyDriverList &device2attach) override
yarp::dev::IControlCalibration * _icontrolcalib
~FineCalibrationChecker()=default
FineCalibrationChecker & operator=(FineCalibrationChecker &&other) noexcept=default
yarp::dev::IEncoders * _ienc
yarp::dev::IMultipleWrapper * _imultwrap