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>
46using namespace yarp::os;
47using namespace yarp::sig;
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);
146 bool doesBaselineExceed(
unsigned int &taxelIndex,
double &baseline,
double &initialBaseline);
160 bool setTaxelPose(
unsigned int taxelId,
const Vector &pose);
201 std::stringstream ss;
Class that encloses everything relate to a skinPart.
Vector getPoseConfidences()
bool setTaxelPoses(const vector< Vector > &poses)
bool setTaxelPose(unsigned int taxelId, const Vector &pose)
bool setTaxelPosesFromFile(const char *filePath)
vector< Vector > getTaxelOrientations()
vector< Vector > getTaxelPoses()
bool setSmoothFactor(float value)
bool setTaxelOrientations(const vector< Vector > &orientations)
bool readRawAndWriteCompensatedData()
bool setTaxelPosition(unsigned int taxelId, const Vector &position)
double getContactCompensationGain()
double getPoseConfidence(unsigned int taxelId)
bool setAddThreshold(unsigned int thr)
string getInputPortName()
bool setContactCompensationGain(double gain)
unsigned int getNumTaxels()
unsigned int getLinkNum()
bool doesBaselineExceed(unsigned int &taxelIndex, double &baseline, double &initialBaseline)
unsigned int getAddThreshold()
bool setTaxelPosesFromFileOld(const char *filePath)
void setSkinPart(SkinPart _skinPart)
vector< Vector > getTaxelPositions()
void setBinarization(bool value)
Vector getTaxelPosition(unsigned int taxelId)
bool setTaxelOrientation(unsigned int taxelId, const Vector &orientation)
void setSmoothFilter(bool value)
bool setTaxelPositions(const Vector &positions)
void calibrationDataCollection()
double getCompensationGain()
Vector getTaxelPose(unsigned int taxelId)
Vector getTouchThreshold()
skinContactList getContacts()
bool setCompensationGain(double gain)
bool setMaxNeighborDistance(double d)
Vector getTaxelOrientation(unsigned int taxelId)
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.