31 #include <yarp/sig/Vector.h>
32 #include <yarp/os/BufferedPort.h>
33 #include <yarp/os/PeriodicThread.h>
34 #include <yarp/os/ResourceFinder.h>
35 #include <yarp/os/Stamp.h>
36 #include <yarp/os/Log.h>
37 #include <yarp/dev/IAnalogSensor.h>
38 #include <yarp/dev/PolyDriver.h>
46 using namespace yarp::os;
47 using namespace yarp::sig;
53 namespace skinManager{
59 static const int MAX_READ_ERROR = 100;
60 static const int MAX_SKIN = 255;
61 static const int MIN_TOUCH_THR = 1;
62 static const double BIN_TOUCH;
63 static const double BIN_NO_TOUCH;
74 vector< list<int> > neighborsXtaxel;
75 vector<Vector> taxelPos;
76 vector<Vector> taxelOri;
77 Vector taxelPoseConfidence;
82 vector<bool> touchDetected;
83 vector<bool> touchDetectedFilt;
84 vector<bool> subTouchDetected;
86 Vector touchThresholds;
87 mutex touchThresholdSem;
88 Vector initialBaselines;
90 Vector compensatedData;
91 Vector compensatedDataOld;
92 Vector compensatedDataFilt;
96 vector<float> start_sum;
97 vector< vector<int> > skin_empty;
100 IAnalogSensor* tactileSensor;
101 PolyDriver* tactileSensorDevice;
102 std::string tactileSensorDevice_inputPortName;
106 int readErrorCounter;
107 vector<unsigned int> saturatedTaxels;
110 unsigned int addThreshold;
111 double compensationGain;
112 double contactCompensationGain;
118 mutex smoothFactorSem;
121 BufferedPort<Vector> compensatedTactileDataPort;
122 BufferedPort<Bottle>* infoPort;
123 BufferedPort<Vector> inputPort;
128 bool init(
string name,
string robotName,
string outputPortName,
string inputPortName);
129 bool readInputData(Vector& skin_values);
130 void sendInfoMsg(
string msg);
131 void computeNeighbors();
132 void updateNeighbors(
unsigned int taxelId);
136 Compensator(
string name,
string robotName,
string outputPortName,
string inputPortName, BufferedPort<Bottle>* _infoPort,
137 double _compensationGain,
double _contactCompensationGain,
int addThreshold,
float _minBaseline,
bool _zeroUpRawData,
138 bool _binarization,
bool _smoothFilter,
float _smoothFactor,
unsigned int _linkId = 0);
141 void calibrationInit();
142 void calibrationDataCollection();
143 void calibrationFinish();
144 bool readRawAndWriteCompensatedData();
145 void updateBaseline();
146 bool doesBaselineExceed(
unsigned int &taxelIndex,
double &baseline,
double &initialBaseline);
151 void setSmoothFilter(
bool value);
152 bool setSmoothFactor(
float value);
153 bool setAddThreshold(
unsigned int thr);
154 bool setCompensationGain(
double gain);
155 bool setContactCompensationGain(
double gain);
156 bool setMaxNeighborDistance(
double d);
157 bool setTaxelPosesFromFile(
const char *filePath);
158 bool setTaxelPosesFromFileOld(
const char *filePath);
159 bool setTaxelPoses(
const vector<Vector> &poses);
160 bool setTaxelPose(
unsigned int taxelId,
const Vector &pose);
161 bool setTaxelPositions(
const Vector &positions);
162 bool setTaxelPosition(
unsigned int taxelId,
const Vector &position);
163 bool setTaxelOrientations(
const vector<Vector> &orientations);
164 bool setTaxelOrientation(
unsigned int taxelId,
const Vector &orientation);
165 void setSkinPart(
SkinPart _skinPart);
167 Vector getTouchThreshold();
170 float getSmoothFactor();
174 Vector getTaxelPosition(
unsigned int taxelId);
175 vector<Vector> getTaxelPositions();
176 Vector getTaxelOrientation(
unsigned int taxelId);
177 vector<Vector> getTaxelOrientations();
178 Vector getTaxelPose(
unsigned int taxelId);
179 vector<Vector> getTaxelPoses();
180 double getPoseConfidence(
unsigned int taxelId);
181 Vector getPoseConfidences();
182 unsigned int getNumTaxels();
183 Vector getCompensation();
201 std::stringstream ss;
Class that encloses everything relate to a skinPart.
double getContactCompensationGain()
string getInputPortName()
unsigned int getLinkNum()
unsigned int getAddThreshold()
void setBinarization(bool value)
double getCompensationGain()
const std::string SkinPart_s[]
const std::string BodyPart_s[]
std::string toString(const T &t)
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.