iCub-main
skinManager.cpp
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
4  * Authors: Andrea Del Prete, Alexander Schmitz
5  * email: andrea.delprete@iit.it, alexander.schmitz@iit.it
6  * website: www.robotcub.org
7  * Permission is granted to copy, distribute, and/or modify this program
8  * under the terms of the GNU General Public License, version 2 or any
9  * later version published by the Free Software Foundation.
10  *
11  * A copy of the license can be found at
12  * http://www.robotcub.org/icub/license/gpl.txt
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
17  * Public License for more details
18  */
19 
20 #include <sstream> // string stream
21 #include <yarp/os/Log.h>
22 #include <yarp/os/LogStream.h>
24 
25 using std::string;
26 using std::vector;
27 
28 using yarp::os::Bottle;
29 using yarp::sig::Vector;
30 
31 using namespace iCub::skinManager;
32 
33 // module default values
34 const int skinManager::MIN_BASELINE_DEFAULT = 3;
35 const int skinManager::ADD_THRESHOLD_DEFAULT = 2;
36 const int skinManager::PERIOD_DEFAULT = 50;
37 const bool skinManager::SMOOTH_FILTER_DEFAULT = false;
38 const float skinManager::SMOOTH_FACTOR_DEFAULT = 0.5;
39 const float skinManager::COMPENSATION_GAIN_DEFAULT = 0.2f;
40 const float skinManager::CONTACT_COMPENSATION_GAIN_DEFAULT = 0.0f;
41 const string skinManager::MODULE_NAME_DEFAULT = "skinManager";
42 const string skinManager::ROBOT_NAME_DEFAULT = "icub";
43 const bool skinManager::ZERO_UP_RAW_DATA_DEFAULT = false;
44 const bool skinManager::BINARIZATION_DEFAULT = false;
45 const string skinManager::RPC_PORT_DEFAULT = "/rpc";
46 
47 bool skinManager::configure(yarp::os::ResourceFinder &rf) {
48  /* Process all parameters from both command-line and .ini file */
49 
50  /* get the module name which will form the stem of all module port names */
51  moduleName = rf.check("name", Value(MODULE_NAME_DEFAULT.c_str()), "module name (string)").asString();
52  robotName = rf.check("robot", Value(ROBOT_NAME_DEFAULT.c_str()), "name of the robot (string)").asString();
53  /* before continuing, set the module name before getting any other parameters,
54  * specifically the port names which are dependent on the module name*/
55  setName(moduleName.c_str());
56 
57  /* get some other values from the configuration file */
58  int period = (int)rf.check("period", Value(PERIOD_DEFAULT),
59  "Period of the thread in ms (positive int)").asInt32();
60  float minBaseline = (float)rf.check("minBaseline", Value(MIN_BASELINE_DEFAULT),
61  "If the baseline reaches this value then, if allowed, a calibration is executed (float in [0,255])").asFloat64();
62  float compGain = (float)rf.check("compensationGain", Value(COMPENSATION_GAIN_DEFAULT),
63  "Gain of the compensation algorithm (float)").asFloat64();
64  float contCompGain = (float)rf.check("contactCompensationGain", Value(CONTACT_COMPENSATION_GAIN_DEFAULT),
65  "Gain of the compensation algorithm during contact (float)").asFloat64();
66  int addThreshold = (int)rf.check("addThreshold", Value(ADD_THRESHOLD_DEFAULT),
67  "Value added to all the touch thresholds (positive int)").asInt32();
68 
69  bool zeroUpRawData = rf.check("zeroUpRawData", Value(ZERO_UP_RAW_DATA_DEFAULT),
70  "if true the raw data are considered from zero up, otherwise from 255 down (bool)").asBool();
71  bool smoothFilter = rf.check("smoothFilter", Value(SMOOTH_FILTER_DEFAULT),
72  "if true then the smoothing filter is active (bool)").asBool();
73  float smoothFactor = (float) rf.check("smoothFactor", Value(SMOOTH_FACTOR_DEFAULT),
74  "Determine the smoothing intensity (float in [0,1])").asFloat64();
75  bool binarization = rf.check("binarization", Value(BINARIZATION_DEFAULT),
76  "if true then the binarization is active (bool)").asBool();
77 
78  /*
79  * attach a port of the same name as the module (prefixed with a /) to the module
80  * so that messages received from the port are redirected to the respond method
81  */
82  string handlerPortName = "/";
83  handlerPortName += getName(rf.check("handlerPort", Value(RPC_PORT_DEFAULT.c_str())).asString());
84  if (!handlerPort.open(handlerPortName.c_str())) {
85  cout << getName() << ": Unable to open port " << handlerPortName << endl;
86  return false;
87  }
88  attach(handlerPort); // attach to port
89  handlerPort.setRpcMode(true);
90 
91 
92  /* ****** Threads ****** */
93  // Compensator thread
94  /* create the thread and pass pointers to the module parameters */
95  myThread = new CompensationThread(moduleName, &rf, robotName, compGain, contCompGain, addThreshold, minBaseline,
96  zeroUpRawData, period, binarization, smoothFilter, smoothFactor);
97  /* now start the thread to do the work */
98  if(!myThread->start()) { // this calls threadInit() and it if returns true, it then calls run()
99  yError() << "[SkinManager] Could not start the compensator thread.";
100  return false;
101  }
102 
103  // Skin diagnostics thread
104  thSkinDiagnostics = new SkinDiagnosticsReadThread(20, rf);
105  if (!thSkinDiagnostics->start()) {
106  yError() << "[SkinManager] Could not start the skin diagnostics thread.";
107  return false;
108  }
109  return true;
110 }
111 
112 
113 bool skinManager::interruptModule()
114 {
115  handlerPort.interrupt();
116 
117  return true;
118 }
119 
120 
121 bool skinManager::close()
122 {
123  /* stop the threads */
124  if(myThread) {
125  myThread->stop();
126  delete myThread;
127  }
128  if (thSkinDiagnostics) {
129  thSkinDiagnostics->stop();
130  delete thSkinDiagnostics;
131  }
132 
133  handlerPort.close();
134 
135  return true;
136 }
137 
138 
139 bool skinManager::respond(const Bottle& command, Bottle& reply) {
140  stringstream temp;
141  reply.clear();
142 
143  SkinManagerCommand com;
144  Bottle params;
145  if(command.get(0).isInt32()){
146  // if first value is int then it is the id of the command
147  com = (SkinManagerCommand)command.get(0).asInt32();
148  yInfo("[rpc] Command received: %s", SkinManagerCommandList[com].c_str());
149  params = command.tail();
150  }
151  else if(!identifyCommand(command, com, params)){
152  reply.addString("Unknown command. Input 'help' to get a list of the available commands.");
153  return true;
154  }
155 
156  switch( com ){
157  case quit:
158  reply.addString("quitting");
159  return false;
160 
161  case help:
162  reply.addVocab32("many"); // print every string added to the bottle on a new line
163  reply.addString((string(getName().c_str()) + " commands are: ").c_str());
164  for(unsigned int i=0; i< SkinManagerCommandSize; i++){
165  reply.addString( ("- "+SkinManagerCommandList[i]+": "+SkinManagerCommandDesc[i]).c_str() );
166  }
167  return true;
168 
169  case calibrate:
170  myThread->calibrate();
171  break;
172 
173  case get_touch_thr:
174  {
175  Vector touchThreshold = myThread->getTouchThreshold();
176  for(size_t i=0; i< touchThreshold.size(); i++)
177  reply.addFloat64(touchThreshold[i]);
178  return true;
179  }
180 
181  case set_binarization:
182  {
183  if(params.size()<1){
184  reply.addString("Binarization state missing! Specify either on or off.");
185  return true;
186  }
187  string value = params.get(0).asString().c_str();
188  if(value.compare("on")==0)
189  myThread->setBinarization(true);
190  else if(value.compare("off")==0)
191  myThread->setBinarization(false);
192  else{
193  reply.addString("Value not recognized.");
194  return true;
195  }
196  break;
197  }
198 
199  case get_binarization:
200  if(myThread->getBinarization())
201  reply.addString("on");
202  else
203  reply.addString("off");
204  return true;
205 
206  case set_smooth_filter:
207  {
208  if(params.size()<1){
209  reply.addString("Smooth filter state missing! Specify either on or off.");
210  return true;
211  }
212  string value = params.get(0).asString().c_str();
213  if(value.compare("on")==0)
214  myThread->setSmoothFilter(true);
215  else if(value.compare("off")==0)
216  myThread->setSmoothFilter(false);
217  else{
218  reply.addString("Value not recognized.");
219  return true;
220  }
221  break;
222  }
223 
224  case get_smooth_filter:
225  if(myThread->getSmoothFilter())
226  reply.addString("on");
227  else
228  reply.addString("off");
229  return true;
230 
231  case set_smooth_factor:
232  {
233  if(params.size()<1 || (!params.get(0).isFloat64() && !params.get(0).isInt32())){
234  reply.addString("New smooth factor value missing or not a number! Smooth factor not updated.");
235  return true;
236  }
237 
238  stringstream temp;
239  if(myThread->setSmoothFactor((float)params.get(0).asFloat64())){
240  temp<< "New smooth factor set: "<< params.get(0).asFloat64();
241  }
242  else{
243  temp<< "ERROR in setting new smooth factor: "<< params.get(0).asFloat64();
244  }
245  reply.addString( temp.str().c_str());
246  return true;
247  }
248 
249  case get_smooth_factor:
250  reply.addFloat64(myThread->getSmoothFactor());
251  return true;
252 
253  case set_threshold:
254  {
255  if(params.size()<1 || (!params.get(0).isInt32())){
256  reply.addString("New threshold value missing or not an integer! Threshold not updated.");
257  return true;
258  }
259 
260  stringstream temp;
261  if(myThread->setAddThreshold(params.get(0).asInt32())){
262  temp<< "New threshold set: "<< params.get(0).asInt32();
263  }
264  else{
265  temp<< "ERROR in setting new threshold: "<< params.get(0).asInt32();
266  }
267  reply.addString( temp.str().c_str());
268  return true;
269  }
270 
271  case get_threshold:
272  reply.addInt32(myThread->getAddThreshold());
273  return true;
274 
275  case set_gain:
276  {
277  if(params.size()<1 || (!params.get(0).isFloat64())){
278  reply.addString("New gain value missing or not a double! Gain not updated.");
279  return true;
280  }
281 
282  stringstream temp;
283  if(myThread->setCompensationGain(params.get(0).asFloat64())){
284  temp<< "New gain set: "<< params.get(0).asFloat64();
285  }
286  else{
287  temp<< "ERROR in setting new gain: "<< params.get(0).asFloat64();
288  }
289  reply.addString( temp.str().c_str());
290  return true;
291  }
292 
293  case get_gain:
294  reply.addFloat64(myThread->getCompensationGain());
295  return true;
296 
297  case set_cont_gain:
298  {
299  if(params.size()<1 || (!params.get(0).isFloat64())){
300  reply.addString("New gain value missing or not a double! Contact gain not updated.");
301  return true;
302  }
303 
304  stringstream temp;
305  if(myThread->setContactCompensationGain(params.get(0).asFloat64())){
306  temp<< "New contact gain set: "<< params.get(0).asFloat64();
307  }
308  else{
309  temp<< "ERROR in setting new contact gain: "<< params.get(0).asFloat64();
310  }
311  reply.addString( temp.str().c_str());
312  return true;
313  }
314 
315  case get_cont_gain:
316  reply.addFloat64(myThread->getContactCompensationGain());
317  return true;
318 
319  case get_max_neigh_dist:
320  reply.addFloat64(myThread->getMaxNeighborDistance());
321  return true;
322 
323  case set_max_neigh_dist:
324  {
325  if(params.size()<1 || (!params.get(0).isFloat64())){
326  reply.addString("New max neighbor distance value missing or not a double! Not updated.");
327  return true;
328  }
329 
330  stringstream temp;
331  if(myThread->setMaxNeighborDistance(params.get(0).asFloat64())){
332  temp<< "New max neighbor distance set: "<< params.get(0).asFloat64();
333  }
334  else{
335  temp<< "ERROR in setting new max neighbor distance: "<< params.get(0).asFloat64();
336  }
337  reply.addString( temp.str().c_str());
338  return true;
339  }
340 
341  case get_skin_parts:
342  {
343  vector<SkinPart> spl = myThread->getSkinParts();
344  for(vector<SkinPart>::const_iterator it=spl.begin(); it!=spl.end(); it++){
345  reply.addInt32(*it);
346  reply.addString(SkinPart_s[*it].c_str());
347  }
348  return true;
349  }
350 
351  case enable_skin_part:
352  if(!(params.size()>0 && params.get(0).isInt32())){
353  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
354  return true;
355  }
356  if(myThread->enableSkinPart((SkinPart)params.get(0).asInt32()))
357  reply.addString("SkinPart enabled");
358  else
359  reply.addString("SkinPart not found");
360  return true;
361 
362  case disable_skin_part:
363  if(!(params.size()>0 && params.get(0).isInt32())){
364  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
365  return true;
366  }
367  if(myThread->disableSkinPart((SkinPart)params.get(0).asInt32()))
368  reply.addString("SkinPart disabled");
369  else
370  reply.addString("SkinPart not found");
371  return true;
372 
373  case is_skin_enabled:
374  if(!(params.size()>0 && params.get(0).isInt32())){
375  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
376  return true;
377  }
378  if(myThread->isSkinEnabled((SkinPart)params.get(0).asInt32()))
379  reply.addString("yes");
380  else
381  reply.addString("no");
382  return true;
383 
384  case is_calibrating:
385  if(myThread->isCalibrating())
386  reply.addString("yes");
387  else
388  reply.addString("no");
389  return true;
390 
391  case get_pose:
392  {
393  if(!(params.size()>0 && params.get(0).isInt32())){
394  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
395  return true;
396  }
397  SkinPart sp = (SkinPart) params.get(0).asInt32();
398  if(params.size()>1 && params.get(1).isInt32()){
399  unsigned int taxelId = params.get(1).asInt32();
400  Vector res = myThread->getTaxelPose(sp, taxelId);
401  if(res.size()>0)
402  addToBottle(reply, res);
403  else
404  reply.addString("No poses for the specified skin part");
405  }
406  else{
407  vector<Vector> res = myThread->getTaxelPoses(sp);
408  addToBottle(reply, res);
409  }
410  return true;
411  }
412 
413  case set_pose:
414  {
415  if(!(params.size()>6 && params.get(0).isInt32() )){
416  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
417  return true;
418  }
419  SkinPart sp = (SkinPart) params.get(0).asInt32();
420  if(params.get(1).isInt32()){
421  unsigned int taxelId = params.get(1).asInt32();
422  Vector pose;
423  if(!bottleToVector(params.tail().tail(), pose)){
424  reply.addString("ERROR while reading the taxel pose");
425  return true;
426  }
427  if(myThread->setTaxelPose(sp, taxelId, pose))
428  reply.addInt32(skin_manager_ok);
429  else{
430  reply.addInt32(skin_manager_error);
431  reply.addString("ERROR: pose was not set");
432  }
433  }
434  else{
435  Vector poses;
436  if(!bottleToVector(params.tail(), poses)){
437  reply.addString("ERROR while reading the taxel poses");
438  return true;
439  }
440  if(myThread->setTaxelPoses(sp, poses))
441  reply.addInt32(skin_manager_ok);
442  else{
443  reply.addInt32(skin_manager_error);
444  reply.addString("ERROR: pose was not set");
445  }
446  }
447  return true;
448  }
449  case get_position:
450  {
451  if(!(params.size()>0 && params.get(0).isInt32())){
452  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
453  return true;
454  }
455  SkinPart sp = (SkinPart) params.get(0).asInt32();
456  if(params.size()>1 && params.get(1).isInt32()){
457  unsigned int taxelId = params.get(1).asInt32();
458  Vector res = myThread->getTaxelPosition(sp, taxelId);
459  if(res.size()>0)
460  addToBottle(reply, res);
461  else
462  reply.addString("No position for the specified skin part");
463  }
464  else{
465  vector<Vector> res = myThread->getTaxelPositions(sp);
466  if(res.empty())
467  reply.addString("Specified skin part has not been found.");
468  else
469  addToBottle(reply, res);
470  }
471  return true;
472  }
473  case set_position:
474  {
475  if(!(params.size()>3 && params.get(0).isInt32() )){
476  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
477  return true;
478  }
479  SkinPart sp = (SkinPart) params.get(0).asInt32();
480  if(params.get(1).isInt32()){
481  unsigned int taxelId = params.get(1).asInt32();
482  Vector pose;
483  if(!bottleToVector(params.tail().tail(), pose)){
484  reply.addString("ERROR while reading the taxel position");
485  return true;
486  }
487  if(myThread->setTaxelPosition(sp, taxelId, pose))
488  reply.addInt32(skin_manager_ok);
489  else{
490  reply.addInt32(skin_manager_error);
491  reply.addString("ERROR: position was not set");
492  }
493  }
494  else{
495  Vector poses;
496  if(!bottleToVector(params.tail(), poses)){
497  reply.addString("ERROR while reading the taxel positions");
498  return true;
499  }
500  if(myThread->setTaxelPositions(sp, poses))
501  reply.addInt32(skin_manager_ok);
502  else{
503  reply.addInt32(skin_manager_error);
504  reply.addString("ERROR: position was not set");
505  }
506  }
507  return true;
508  }
509  case get_confidence:
510  {
511  if(!(params.size()>0 && params.get(0).isInt32())){
512  reply.addString(("ERROR: SkinPart is not specified. Params read are: "+string(params.toString().c_str())).c_str());
513  return true;
514  }
515  SkinPart sp = (SkinPart) params.get(0).asInt32();
516  if(params.size()>1 && params.get(1).isInt32()){
517  unsigned int taxelId = params.get(1).asInt32();
518  double res = myThread->getPoseConfidence(sp, taxelId);
519  reply.addFloat64(res);
520  }
521  else{
522  Vector res = myThread->getPoseConfidences(sp);
523  if(res.size()==0)
524  reply.addString("Specified skin part has not been found");
525  else
526  addToBottle(reply, res);
527  }
528  return true;
529 
530  }
531  case get_info:
532  reply.append(myThread->getInfo());
533  return true;
534 
535  default:
536  reply.addString("ERROR: This command is known but it is not managed in the code.");
537  return true;
538  }
539 
540  reply.addString( (SkinManagerCommandList[com]+" command received.").c_str());
541 
542  return true;
543 }
544 
545 void skinManager::addToBottle(Bottle& b, const Vector& v){
546  for(unsigned int i=0; i<v.size(); i++)
547  b.addFloat64(v[i]);
548 }
549 
550 void skinManager::addToBottle(Bottle& b, const vector<Vector>& v){
551  for(unsigned int i=0; i<v.size(); i++)
552  for(unsigned int j=0; j<v[i].size(); j++)
553  b.addFloat64(v[i][j]);
554 }
555 
556 bool skinManager::bottleToVector(const yarp::os::Bottle& b, yarp::sig::Vector& v){
557  for(int i=0; i<b.size(); i++)
558  if(b.get(i).isFloat64() || b.get(i).isInt32())
559  v.push_back(b.get(i).asFloat64());
560  else
561  return false;
562  return true;
563 }
564 
568 bool skinManager::identifyCommand(Bottle commandBot, SkinManagerCommand &com, Bottle& params) {
569  for(unsigned int i=0; i<SkinManagerCommandSize; i++){
570  stringstream stream(SkinManagerCommandList[i]);
571  string word;
572  int j=0;
573  bool found = true;
574 
575  while(stream>>word){
576  if (commandBot.get(j).asString() != word.c_str()){
577  found=false;
578  break;
579  }
580  j++;
581  }
582  if(found){
583  com = (SkinManagerCommand)i;
584  params = commandBot.tail();
585  for(int k=1; k<j; k++)
586  params = params.tail();
587  return true;
588  }
589  }
590 
591  return false;
592 }
593 
594 bool skinManager::updateModule() {
595  double avgTime, stdDev, period;
596  period = myThread->getPeriod();
597  myThread->getEstimatedPeriod(avgTime, stdDev);
598  double avgTimeUsed, stdDevUsed;
599  myThread->getEstimatedUsed(avgTimeUsed, stdDevUsed); // real duration of run()
600  if(avgTime > 1.3 * period){
601  yWarning("Thread too slow. Real period: %3.3f+/-%3.3f. Expected period: %3.3f.\n", avgTime, stdDev, period);
602  yWarning("Duration of 'run' method: %3.3f+/-%3.3f.\n", avgTimeUsed, stdDevUsed);
603  }
604  return true;
605 }
606 
607 double skinManager::getPeriod(){ return 1.0;}
608 
bool setTaxelPosition(SkinPart sp, unsigned int taxelId, const Vector &position)
Vector getTaxelPose(SkinPart sp, unsigned int taxelId)
bool setTaxelPose(SkinPart sp, unsigned int taxelId, const Vector &pose)
double getPoseConfidence(SkinPart sp, unsigned int taxelId)
vector< Vector > getTaxelPoses(SkinPart sp=SKIN_PART_ALL)
bool setTaxelPositions(SkinPart sp, const Vector &positions)
bool setTaxelPoses(SkinPart sp, const vector< Vector > &poses)
Vector getTaxelPosition(SkinPart sp, unsigned int taxelId)
vector< Vector > getTaxelPositions(SkinPart sp=SKIN_PART_ALL)
static int v
Definition: iCub_Sim.cpp:42
const std::string SkinPart_s[]
Definition: common.h:64
const std::string SkinManagerCommandDesc[]
const std::string SkinManagerCommandList[]
unsigned short word
Definition: MS3D.h:75