iCub-main
emotionInterfaceModule.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2007 Alex Bernardino
5  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
6  *
7  */
8 
9 #include <sstream>
10 #include <string>
11 #include <cstdlib>
12 #include <bitset>
13 #include <iomanip>
14 #include <yarp/os/ResourceFinder.h>
15 #include <yarp/dev/IControlMode.h>
16 #include <yarp/dev/IControlLimits.h>
17 #include "emotionInterfaceModule.h"
18 
19 
20 void EmotionInitReport::report(const PortInfo &info) {
21  if ((emo!= nullptr) && info.created && !info.incoming)
22  emo->initEmotion();
23 }
24 
25 std::bitset<32> populateBitset(const yarp::os::Bottle& bt)
26 {
27  std::bitset<32> bitSet{};
28  for(size_t i=0; i<bt.size(); i++){
29  bitSet.set(bt.get(i).asInt32());
30  }
31  return bitSet;
32 }
33 
34 std::string getHexCode(const Bottle& bot) {
35 
36  if( bot.size() < 2 )
37  {
38  return {};
39  }
40 
41  auto botBits = bot.get(1).asList();
42  if (!botBits || botBits->size() == 0 || botBits->size()>32)
43  {
44  yError()<<"The bitmask has to be defined as a list of <32 integers";
45  return {};
46  }
47  auto bitSet = populateBitset(*botBits);
48  stringstream ss;
49  ss << setfill('0') << setw(8) << hex << uppercase << bitSet.to_ulong();
50  return ss.str();
51 }
52 
53 void printHelp (yarp::os::Bottle& reply) {
54 
55  std::string helpMessage{};
56  helpMessage = helpMessage +
57  " commands are: \n" +
58  "help to display this message\n" +
59  "\n"+
60  "set <part> <emotion> set a specific emotion for a defined part \n" +
61  "\tthe available part are: mou, eli, leb, reb, all\n"+
62  "\tthe available emotions are: neu, hap, sad, sur, ang, evi, shy, cun\n"+
63  "\n"+
64  "set col <color> set the color of the led\n" +
65  "\t!! available only for rfe board !!\n"+
66  "\tthe available colors are: black, white, red, lime, blue, yellow,"
67  " cyan, magenta, silver, gray, maroon, olive, green, purple, teal, navy\n"+
68  "\n"+
69  "set brig <brig> set the brightness of the leds\n"+
70  "\t!! available only for rfe board !!\n"+
71  "\tthe brightness is defined by an integer from 0 to 5, where 0 means led off\n"+
72  "\n"+
73  "set mask (<col_leb> <m_name_leb> <brig_leb>) (<col_reb> <m_name_reb> <brig_reb>) (<col_mou> <m_name_mou> <brig_mou>) set color, bitmask and brightness for each part(leb, reb, mou)\n"+
74  "\t!! available only for rfe board !!\n"+
75  "\tm_name stands for mask name and the availble bitmasks can be consulted/added in faceExpressions/emotions_rfe.ini\n";
76 
77  reply.clear();
78  reply.addVocab32("many");
79  reply.addString(helpMessage.c_str());
80 }
81 
83 }
84 
85 bool EmotionInterfaceModule::configure(ResourceFinder& config){
86 
87  std::string modName = config.find("name").asString();
88  setName(modName.c_str());
89 
90  _lasttime = Time::now();
91  if (config.check("help","if present, display usage message")) {
92  yError("Call with --name /module_prefix --file configFile.ini \n");
93  return false;
94  }
95 
96  _highlevelemotions = config.check("emotions", Value(0), "Number of predefined facial expressions").asInt32();
97  _numberOfColors = config.check("colors", Value(0), "Number of predefined colors").asInt32();
98  _eyebrowmaskemotions = config.check("bitmask_eyebrow_emotions", Value(0), "Number of predefined bitmask eyebrow expressions").asInt32();
99  _mouthmaskemotions = config.check("bitmask_mouth_emotions", Value(0), "Number of predefined bitmask eyebrow expressions").asInt32();
100  _auto = config.check("auto");
101  _period = config.check("period", Value(10.0), "Period for expression switching in auto mode").asFloat64();
102  if(_highlevelemotions == 0)
103  {
104  _emotion_table = nullptr;
105  }
106  else //allocate space for facial expression codes
107  {
108  _emotion_table = new EM_CODE[_highlevelemotions];
109  if(!_emotion_table)
110  {
111  yError("Memory allocation problem\n");
112  return false;
113  }
114  for(int i = 0; i < _highlevelemotions; i++)
115  {
116  std::ostringstream ss;
117  ss << "E" << i+1;
118  std::string name = ss.str();
119  if(!config.check(name))
120  {
121  yError("Missing identifier %s.", name.c_str());
122  return false;
123  }
124  else
125  {
126  Bottle& bot = config.findGroup(name);
127  if( bot.size() < 6 )
128  {
129  printf("Invalid parameter list for identifier %s.", name.c_str());
130  return false;
131  }
132  //first field - name of the expression
133  std::string n1 = bot.get(1).toString();
134 
135  if(n1.length()!=3) //must have length 3
136  {
137  yError("First field of identifier %s has invalid size (must be 3).", name.c_str());
138  return false;
139  }
140  else
141  {
142  const char *buffer = n1.c_str();
143  strncpy(_emotion_table[i].name, buffer, 3);
144  }
145 
146  //second field - command to left eyebrow
147  std::string n2 = bot.get(2).toString();
148  const char * sfd = n2.c_str();
149  if(n2.length()!=3) //must have length 3
150  {
151  yError("Second field of identifier %s has invalid size (must be 3).", name.c_str());
152  return false;
153  }
154  else
155  {
156  const char *buffer = n2.c_str();
157  strncpy(_emotion_table[i].leb, buffer, 2);
158  }
159  //third field - command to right eyebrow
160  std::string n3 = bot.get(3).toString();
161  if(n3.length()!=3) //must have length 3
162  {
163  yError("Third field of identifier %s has invalid size (must be 3).", name.c_str());
164  return false;
165  }
166  else
167  {
168  const char *buffer = n3.c_str();
169  strncpy(_emotion_table[i].reb, buffer, 2);
170  }
171 
172  //fourth field - command to mouth
173  std::string n4 = bot.get(4).toString();
174  if(n4.length()!=3) //must have length 3
175  {
176  yError("Fourth field of identifier %s has invalid size (must be 3).", name.c_str());
177  return false;
178  }
179  else
180  {
181  const char *buffer = n4.c_str();
182  strncpy(_emotion_table[i].mou, buffer, 2);
183  }
184 
185  //fifth field - command to eyelids
186  std::string n5 = bot.get(5).toString();
187  if(n5.length()!=3) //must have length 3
188  {
189  yError("Fifth field of identifier %s has invalid size (must be 3).", name.c_str());
190  return false;
191  }
192  else
193  {
194  const char *buffer = n5.c_str();
195  strncpy(_emotion_table[i].eli, buffer, 2);
196  }
197  }
198  }
199  }
200  if(_numberOfColors) {
201  std::string color_id;
202  for(size_t index=0; index<_numberOfColors; index++){
203  color_id = "C"+std::to_string(index);
204  Bottle& bot = config.findGroup(color_id);
205  if( bot.size() < 3 )
206  {
207  yError("Invalid parameter list for the identifier %s.", color_id.c_str());
208  return false;
209  }
210 
211  std::string code = bot.get(2).toString();
212  if (code.length()!=3)
213  {
214  yError("The identifier must have size 3");
215  return false;
216  }
217  _color_table[bot.get(1).asString()] = code.substr(0,code.length()-1);
218  }
219  }
220  if(_eyebrowmaskemotions) {
221  std::string id;
222  for(size_t index=0; index<_eyebrowmaskemotions; index++){
223  id = "BM_EB"+std::to_string(index);
224  Bottle& bot = config.findGroup(id);
225  auto hexCode = getHexCode(bot);
226  if(hexCode.empty()){
227  yError()<<"Parsing of the bitmap failed";
228  return false;
229  }
230  _bitmask_emotion_table[id] = hexCode;
231  }
232 
233  }
234  if(_mouthmaskemotions) {
235 
236  std::string id;
237  for(size_t index=0; index<_eyebrowmaskemotions; index++){
238  id = "BM_M"+std::to_string(index);
239  Bottle& bot = config.findGroup(id);
240  auto hexCode = getHexCode(bot);
241  if(hexCode.empty()){
242  yError()<<"Parsing of the bitmap failed";
243  return false;
244  }
245  _bitmask_emotion_table[id] = hexCode;
246  }
247 
248  }
249 
250  yarp::os::Property rcb_face_conf{ {"device",Value("remote_controlboard")},
251  {"local", Value(getName("/face/remoteControlBoard"))},
252  {"remote",Value("/icub/face")},
253  {"part",Value("face")} };
254 
255  if (_poly.open(rcb_face_conf))
256  {
257  yarp::dev::IControlMode* iCM{ nullptr };
258  yarp::dev::IControlLimits* iCtrlLim{ nullptr };
259  auto ok = _poly.view(iCM);
260  ok &= _poly.view(_iPos);
261  ok &= _poly.view(iCtrlLim);
262  if (iCM)
263  ok &= iCM->setControlMode(_joint_eylids, VOCAB_CM_POSITION);
264  if (iCtrlLim) {
265  ok &= iCtrlLim->getLimits(_joint_eylids, &_min, &_max);
266  _max = _max - 25.0; // safe zone for avoiding hw fault
267  }
268  if (_iPos) {
269  ok &= _iPos->setRefSpeed(_joint_eylids, 50.0); // max velocity that doesn't give problems
270  ok &= _iPos->setRefAcceleration(_joint_eylids,std::numeric_limits<double>::max());
271  }
272  if (!ok)
273  {
274  yError() << "Fail to configure correctly the remote_controlboard";
275  return false;
276  }
277  }
278  else
279  {
280  yWarning() << "Failed to open the remote_controlboard device for commanding the eyelids, you can ignore this warning if your robot doesn't have the rfe board";
281  }
282 
283  // open ports
284  _inputPort.open(getName("/in"));
285  _outputPort.open(getName("/out"));
286  _outputPort.setReporter(emotionInitReport);
287  _initEmotionTrigger=0;
288  attach(_inputPort);
289 
290  return true;
291 }
292 
294 
295  if(_inputPort.isOpen())
296  _inputPort.close();
297  if(!_outputPort.isClosed())
298  _outputPort.close();
299 
300  if (_emotion_table != nullptr)
301  {
302  delete [] _emotion_table;
303  _emotion_table = nullptr;
304  }
305 
306  if(_poly.isValid())
307  {
308  _poly.close();
309  _iPos = nullptr;
310  }
311 
312  return true;
313 }
314 
316 
317  _inputPort.interrupt();
318  _outputPort.interrupt();
319  return true;
320 }
321 
323  _initEmotionTrigger++;
324 }
325 
327  double curtime;
328  int expr;
329  if( _auto )
330  {
331  curtime = Time::now();
332  if(curtime - _lasttime > _period) //change expression
333  {
334  _lasttime = curtime;
335  expr = rand() % _highlevelemotions;
336  std::string cmd(_emotion_table[expr].name);
337  setAll(cmd);
338  }
339  }
340  else if (_initEmotionTrigger>0)
341  {
342  setAll("hap");
343  _initEmotionTrigger=0;
344  }
345  return true;
346 }
347 
349  return 1.0;
350 }
351 
352 bool EmotionInterfaceModule::respond(const Bottle &command,Bottle &reply){
353 
354  bool ok = false;
355  bool rec = false; // is the command recognized?
356 
357  switch (command.get(0).asVocab32()) {
358  case EMOTION_VOCAB_HELP:
359  printHelp(reply);
360  return true;
361  case EMOTION_VOCAB_SET:
362  rec = true;
363  {
364  switch(command.get(1).asVocab32()) {
365  case EMOTION_VOCAB_MOUTH:{
366  ok = setMouth(command.get(2).toString());
367  break;
368  }
369  case EMOTION_VOCAB_EYELIDS:{
370  ok = setEyelids(command.get(2).toString());
371  break;
372  }
374  ok = setLeftEyebrow(command.get(2).toString());
375  break;
376  }
378  ok = setRightEyebrow(command.get(2).toString());
379  break;
380  }
381  case EMOTION_VOCAB_ALL:{
382  ok = setAll(command.get(2).toString());
383  break;
384  }
385  case EMOTION_VOCAB_RAW:{
386  ok = setRaw(command.get(2).toString());
387  break;
388  }
389  case EMOTION_VOCAB_COLOR:{
390  ok = setColor(command.get(2).toString());
391  break;
392  }
393  case EMOTION_VOCAB_BRIG: {
394  ok = setBrightness(command.get(2).toString());
395  break;
396  }
397  case EMOTION_VOCAB_MASK:{
398  ok = setMask(command);
399  break;
400  }
401  default:
402  cout << "received an unknown request after a VOCAB_SET" << endl;
403  break;
404  }
405  }
406  break;
407  case EMOTION_VOCAB_GET:
408  rec = true;
409  /*{
410  reply.addVocab32(VOCAB_IS);
411  reply.add(command.get(1));
412  switch(command.get(1).asVocab32()) {
413  case EGOSPHERE_VOCAB_THRESHOLD_SALIENCE:{
414  float thr = getThresholdSalience();
415  reply.addFloat64((double)thr);
416  ok = true;
417  break;
418  }
419  case EGOSPHERE_VOCAB_OUTPUT:{
420  int v = (int)getOutput();
421  reply.addInt32(v);
422  ok = true;
423  break;
424  }
425  case EGOSPHERE_VOCAB_SALIENCE_DECAY:{
426  double rate = getSalienceDecay();
427  reply.addFloat64(rate);
428  ok = true;
429  break;
430  }
431  break;
432  default:
433  cout << "received an unknown request after a VOCAB_GET" << endl;
434  break;
435  }
436  }
437  */
438  break;
439 
440  }
441 
442  if (!rec)
443  ok = false;
444 
445  if (!ok) {
446  reply.clear();
447  reply.addVocab32(EMOTION_VOCAB_FAILED);
448  }
449  else
450  reply.addVocab32(EMOTION_VOCAB_OK);
451 
452  return ok;
453 }
454 
455 //get the index in _emotions_table of a emotion name
456 int EmotionInterfaceModule::getIndex(const std::string cmd)
457 {
458  if(_highlevelemotions == 0)
459  return -1;
460 
461  int i;
462  for(i = 0; i < _highlevelemotions; i++)
463  {
464  if(strncmp(_emotion_table[i].name, cmd.c_str(), 3) == 0) //strings identical
465  break;
466  }
467 
468  if( i == _highlevelemotions ) // no match
469  return -1;
470 
471  return i;
472 }
473 
474 //send the actual string to the port
475 bool EmotionInterfaceModule::writePort(const char* cmd)
476 {
477  Bottle &btmp = _outputPort.prepare();
478  btmp.clear();
479  btmp.addString(cmd);
480  _outputPort.write(true);
481  Time::delay(0.001);
482  return true;
483 }
484 
485 
486 // interface functions
488 {
489  char cmdbuffer[] = {0,0,0,0};
490  int i;
491  i = getIndex(cmd);
492  if(i < 0)
493  return false;
494 
495  if( _emotion_table[i].leb[0] == '*' || _emotion_table[i].leb[1] == '*')
496  return true; //leave it in the same state
497 
498  cmdbuffer[0]= 'L';
499  cmdbuffer[1]=_emotion_table[i].leb[0];
500  cmdbuffer[2]=_emotion_table[i].leb[1];
501  writePort(cmdbuffer);
502  return true;
503 }
504 
506 {
507  char cmdbuffer[] = {0,0,0,0};
508  int i;
509  i = getIndex(cmd);
510  if(i < 0)
511  return false;
512 
513  if( _emotion_table[i].reb[0] == '*' || _emotion_table[i].reb[1] == '*')
514  return true; //leave it in the same state
515 
516  cmdbuffer[0]= 'R';
517  cmdbuffer[1]=_emotion_table[i].reb[0];
518  cmdbuffer[2]=_emotion_table[i].reb[1];
519  writePort(cmdbuffer);
520  return true;
521 }
522 
523 bool EmotionInterfaceModule::setMouth(const std::string cmd)
524 {
525  char cmdbuffer[] = {0,0,0,0};
526  int i;
527  i = getIndex(cmd);
528  if(i < 0)
529  return false;
530 
531  if( _emotion_table[i].mou[0] == '*' || _emotion_table[i].mou[1] == '*')
532  return true; //leave it in the same state
533 
534  cmdbuffer[0]= 'M';
535  cmdbuffer[1]=_emotion_table[i].mou[0];
536  cmdbuffer[2]=_emotion_table[i].mou[1];
537  writePort(cmdbuffer);
538  return true;
539 }
540 
542 {
543  char cmdbuffer[] = {0,0,0,0};
544  const auto i = getIndex(cmd);
545  if(i < 0)
546  return false;
547 
548  auto res{ true };
549 
550  if( _emotion_table[i].eli[0] == '*' || _emotion_table[i].eli[1] == '*')
551  return true; //leave it in the same state
552  if (_iPos)
553  {
554  auto target_pos = getEyelidsTarget(_emotion_table[i].eli[0], _emotion_table[i].eli[1]);
555  res = _iPos->positionMove(_joint_eylids, target_pos);
556  }
557  else
558  {
559  cmdbuffer[0] = 'S';
560  cmdbuffer[1] = _emotion_table[i].eli[0];
561  cmdbuffer[2] = _emotion_table[i].eli[1];
562  res = writePort(cmdbuffer);
563  }
564 
565  return res;
566 }
567 
568 bool EmotionInterfaceModule::setAll(const std::string cmd)
569 {
572  setMouth(cmd);
573  setEyelids(cmd);
574  return true;
575 }
576 
577 bool EmotionInterfaceModule::setRaw(const std::string cmd)
578 {
579  bool res{ true };
580  if (cmd.size() == 3 && cmd.at(0) == 'p' && _iPos)
581  {
582  const auto target_pos = getEyelidsTarget(cmd[1], cmd[2]);
583  res = _iPos->positionMove(_joint_eylids, target_pos);
584  }
585  else {
586  res &= writePort(cmd.c_str());
587  }
588  return res;
589 }
590 
591 bool EmotionInterfaceModule::setColor(const std::string& cmd)
592 {
593  char cmdbuffer[] = {0,0,0,0};
594 
595  if(_color_table.find(cmd) == _color_table.end()){
596  yError()<<"Color"<<cmd<<"not available";
597  return false;
598  }
599 
600  cmdbuffer[0]= 'C';
601  cmdbuffer[1]=_color_table[cmd][0];
602  cmdbuffer[2]=_color_table[cmd][1];
603  return writePort(cmdbuffer);
604 }
605 
607 {
608  char cmdbuffer[] = {0,0,0,0};
609 
610  cmdbuffer[0]= 'B';
611  cmdbuffer[1]= '0';
612  cmdbuffer[2]= cmd[0];
613  return writePort(cmdbuffer);
614 }
615 
616 
618 {
619  string cmdbuffer{};
620  if(cmd.size()!=5) {
621  yError()<<"Bad request, it should be set mask (color mask) (color mask) (color mask)";
622  return false;
623  }
624  auto botLeb = cmd.get(2).asList(); // bottle of the left eyebrow
625  auto botReb = cmd.get(3).asList(); // bottle of the right eyebrow
626  auto botMou = cmd.get(4).asList(); // bottle of the mouth eyebrow
627 
628  if(!botLeb || !botReb || !botMou) {
629  yError()<<"Bad request, missing one of the three list";
630  return false;
631  }
632 
633  if(botLeb->size()!=3 || botReb->size()!=3 || botMou->size()!=3) {
634  yError()<<"Bad request, each list has to be (color mask brightness)";
635  return false;
636  }
637  auto colLeb = botLeb->get(0).asString();
638  auto colReb = botReb->get(0).asString();
639  auto colMou = botMou->get(0).asString();
640 
641  auto maskLeb = botLeb->get(1).asString();
642  auto maskReb = botReb->get(1).asString();
643  auto maskMou = botMou->get(1).asString();
644 
645  auto brightLeb = '0'+ botLeb->get(2).toString();
646  auto brightReb = '0'+ botReb->get(2).toString();
647  auto brightMou = '0'+ botMou->get(2).toString();
648 
649  if(_bitmask_emotion_table.find(maskLeb) == _bitmask_emotion_table.end() ||
650  _bitmask_emotion_table.find(maskReb) == _bitmask_emotion_table.end() ||
651  _bitmask_emotion_table.find(maskMou) == _bitmask_emotion_table.end()) {
652  yError()<<"One or more bitmask has not been defined in the ini file";
653  return false;
654  }
655 
656  cmdbuffer += 'Z';
657  // LEB
658  cmdbuffer += _color_table[colLeb];
659  cmdbuffer += brightLeb;
660  cmdbuffer += _bitmask_emotion_table[maskLeb];
661  // REB
662  cmdbuffer += _color_table[colReb];
663  cmdbuffer += brightReb;
664  cmdbuffer += _bitmask_emotion_table[maskReb];
665  // Mouth
666  cmdbuffer += _color_table[colMou];
667  cmdbuffer += brightMou;
668  cmdbuffer += _bitmask_emotion_table[maskMou];
669 
670  return writePort(cmdbuffer.c_str());
671 }
672 
673 double EmotionInterfaceModule::getEyelidsTarget(const char eli0, const char eli1)
674 {
675  std::string percentage_str{ eli0 };
676  percentage_str += '.';
677  percentage_str += eli1;
678  auto percentage = std::stod(percentage_str);
679  if (percentage > 1.0)
680  percentage = 1.0;
681  return (1.0 - percentage) * (_max - _min); // because min-> open, max->closed
682 }
bool setMouth(const std::string cmd) override
bool setRaw(const std::string cmd) override
bool setAll(const std::string cmd) override
bool setEyelids(const std::string cmd) override
bool setLeftEyebrow(const std::string cmd) override
bool setRightEyebrow(const std::string cmd) override
bool configure(ResourceFinder &config) override
bool respond(const Bottle &command, Bottle &reply) override
cmd
Definition: dataTypes.h:30
std::bitset< 32 > populateBitset(const yarp::os::Bottle &bt)
void printHelp(yarp::os::Bottle &reply)
std::string getHexCode(const Bottle &bot)
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_BRIG
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_FAILED
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_SET
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_ALL
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_EYELIDS
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_RIGHTEYEBROW
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_GET
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_LEFTEYEBROW
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_MASK
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_COLOR
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_OK
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_MOUTH
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_HELP
constexpr yarp::conf::vocab32_t EMOTION_VOCAB_RAW
const FSC max
Definition: strain.h:48