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;
105 int readErrorCounter;
106 vector<unsigned int> saturatedTaxels;
109 unsigned int addThreshold;
110 double compensationGain;
111 double contactCompensationGain;
117 mutex smoothFactorSem;
120 BufferedPort<Vector> compensatedTactileDataPort;
121 BufferedPort<Bottle>* infoPort;
122 BufferedPort<Vector> inputPort;
127 bool init(
string name,
string robotName,
string outputPortName,
string inputPortName);
128 bool readInputData(Vector& skin_values);
129 void sendInfoMsg(
string msg);
130 void computeNeighbors();
131 void updateNeighbors(
unsigned int taxelId);
135 Compensator(
string name,
string robotName,
string outputPortName,
string inputPortName, BufferedPort<Bottle>* _infoPort,
136 double _compensationGain,
double _contactCompensationGain,
int addThreshold,
float _minBaseline,
bool _zeroUpRawData,
137 bool _binarization,
bool _smoothFilter,
float _smoothFactor,
unsigned int _linkId = 0);
140 void calibrationInit();
141 void calibrationDataCollection();
142 void calibrationFinish();
143 bool readRawAndWriteCompensatedData();
144 void updateBaseline();
145 bool doesBaselineExceed(
unsigned int &taxelIndex,
double &baseline,
double &initialBaseline);
150 void setSmoothFilter(
bool value);
151 bool setSmoothFactor(
float value);
152 bool setAddThreshold(
unsigned int thr);
153 bool setCompensationGain(
double gain);
154 bool setContactCompensationGain(
double gain);
155 bool setMaxNeighborDistance(
double d);
156 bool setTaxelPosesFromFile(
const char *filePath);
157 bool setTaxelPosesFromFileOld(
const char *filePath);
158 bool setTaxelPoses(
const vector<Vector> &poses);
159 bool setTaxelPose(
unsigned int taxelId,
const Vector &pose);
160 bool setTaxelPositions(
const Vector &positions);
161 bool setTaxelPosition(
unsigned int taxelId,
const Vector &position);
162 bool setTaxelOrientations(
const vector<Vector> &orientations);
163 bool setTaxelOrientation(
unsigned int taxelId,
const Vector &orientation);
164 void setSkinPart(
SkinPart _skinPart);
166 Vector getTouchThreshold();
169 float getSmoothFactor();
173 Vector getTaxelPosition(
unsigned int taxelId);
174 vector<Vector> getTaxelPositions();
175 Vector getTaxelOrientation(
unsigned int taxelId);
176 vector<Vector> getTaxelOrientations();
177 Vector getTaxelPose(
unsigned int taxelId);
178 vector<Vector> getTaxelPoses();
179 double getPoseConfidence(
unsigned int taxelId);
180 Vector getPoseConfidences();
181 unsigned int getNumTaxels();
182 Vector getCompensation();
189 string getInputPortName(){
return tactileSensorDevice->getValue(
"remote").asString().c_str(); }
200 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.