iCub-main
SimulatorModule.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) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Vadim Tikhanoff, Paul Fitzpatrick
6 * email: vadim.tikhanoff@iit.it, paulfitz@alum.mit.edu
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 #include "SimulatorModule.h"
21 
22 #include <yarp/os/Bottle.h>
23 #include <yarp/os/BufferedPort.h>
24 #include <yarp/os/Property.h>
25 #include <yarp/os/Network.h>
26 #include <yarp/os/Os.h>
27 #include <yarp/os/Time.h>
28 #include <yarp/sig/Vector.h>
29 #include <yarp/sig/Image.h>
30 #include <yarp/dev/Drivers.h>
31 #include <yarp/dev/PolyDriver.h>
32 #include <yarp/os/LogStream.h>
33 #include <stdio.h>
34 #include <string.h>
35 
36 #include <string>
37 
38 #ifdef WIN32
39 #include <windows.h>
40 #endif
41 
42 using namespace yarp::os;
43 using namespace yarp::sig;
44 #ifndef OMIT_LOGPOLAR
45 using namespace iCub::logpolar;
46 #endif
47 using namespace std;
48 
49 bool viewParam1 = false, viewParam2 = false;
50 bool noCam;
51 
52 const int ifovea = 128;
53 const int baseWidth = 320;
54 const int baseHeight = 240;
55 
57  Simulation *sim) :
58  moduleName(config.getModuleName()),
59  world_manager(world),
60  robot_config(config),
61  robot_flags(config.getFlags()),
62  finder(config.getFinder()),
63  sim(sim) {
64  Property options;
65  wrappedStep = NULL;
66  stopped = false;
67  extractImages = false;
68  viewParam1 = false;
69  viewParam2 = false;
70  sim = NULL;
71  sloth = 0;
72  iCubLArm = NULL;
73  iCubRArm = NULL;
74  iCubHead = NULL;
75  iCubLLeg = NULL;
76  iCubRLeg = NULL;
77  iCubTorso = NULL;
78  failureToLaunch = false;
79  idesc = NULL;
80  dd_descSrv = NULL;
81  dd_descClnt = NULL;
82 }
83 
85  tactileLeftHandPort.prepare() = report;
86  generalStamp.update(Time::now());
87  tactileLeftHandPort.setEnvelope(generalStamp);
88  tactileLeftHandPort.write();
89 }
90 
92  tactileRightHandPort.prepare() = report;
93  generalStamp.update(Time::now());
94  tactileRightHandPort.setEnvelope(generalStamp);
95  tactileRightHandPort.write();
96 }
97 
99  return tactileLeftHandPort.getOutputCount()>0;
100 }
101 
103  return tactileRightHandPort.getOutputCount()>0;
104 }
105 
106 
107 //whole_body_skin_emul
109  iCub::skinDynLib::skinContactList &skinEvents = skinEventsPort.prepare();
110  skinEvents.clear();
111  skinEvents.insert(skinEvents.end(), skinContactListReport.begin(), skinContactListReport.end());
112  generalStamp.update(Time::now());
113  skinEventsPort.setEnvelope(generalStamp);
114  skinEventsPort.write();
115 }
116 
118  return skinEventsPort.getOutputCount()>0;
119 }
120 
122  tactileLeftArmPort.prepare() = report;
123  generalStamp.update(Time::now());
124  tactileLeftArmPort.setEnvelope(generalStamp);
125  tactileLeftArmPort.write();
126 }
127 
129  tactileRightArmPort.prepare() = report;
130  generalStamp.update(Time::now());
131  tactileRightArmPort.setEnvelope(generalStamp);
132  tactileRightArmPort.write();
133 }
134 
136  return tactileLeftArmPort.getOutputCount()>0;
137 }
138 
140  return tactileRightArmPort.getOutputCount()>0;
141 }
142 
144  tactileLeftForearmPort.prepare() = report;
145  generalStamp.update(Time::now());
146  tactileLeftForearmPort.setEnvelope(generalStamp);
147  tactileLeftForearmPort.write();
148 }
149 
151  tactileRightForearmPort.prepare() = report;
152  generalStamp.update(Time::now());
153  tactileRightForearmPort.setEnvelope(generalStamp);
154  tactileRightForearmPort.write();
155 }
156 
158  return tactileLeftForearmPort.getOutputCount()>0;
159 }
160 
162  return tactileRightForearmPort.getOutputCount()>0;
163 }
164 
165 void SimulatorModule::sendTouchTorso(Bottle& report){
166  tactileTorsoPort.prepare() = report;
167  generalStamp.update(Time::now());
168  tactileTorsoPort.setEnvelope(generalStamp);
169  tactileTorsoPort.write();
170 }
171 
173  return tactileTorsoPort.getOutputCount()>0;
174 }
175 //end of whole_body_skin_emul methods
176 
177 void SimulatorModule::sendInertial(Bottle& report){
178  inertialPort.prepare() = report;
179  generalStamp.update(Time::now());
180  inertialPort.setEnvelope(generalStamp);
181  inertialPort.write();
182 }
183 
185  return inertialPort.getOutputCount()>0;
186 }
187 
189  displayStep(0);
190 }
191 
193  yInfo("Closing module...\n");
194  interruptModule();
195 
196  if (sim != NULL)
197  delete sim;
198  sim = NULL;
199 
200  portLeft.interrupt();
201  portRight.interrupt();
202 #ifndef OMIT_LOGPOLAR
203  portLeftFov.interrupt();
204  portLeftLog.interrupt();
205  portRightFov.interrupt();
206  portRightLog.interrupt();
207 #endif
208  portWide.interrupt();
209 
210  portLeft.close();
211 
212  portRight.close();
213 
214 #ifndef OMIT_LOGPOLAR
215  portLeftFov.close();
216  portLeftLog.close();
217  portRightFov.close();
218  portRightLog.close();
219 #endif
220 
221  portWide.close();
222 
223  tactileLeftHandPort.close();
224  tactileRightHandPort.close();
225  tactileLeftArmPort.close();
226  tactileRightArmPort.close();
227  tactileLeftForearmPort.close();
228  tactileRightForearmPort.close();
229  tactileTorsoPort.close();
230 
231  inertialPort.close();
232  cmdPort.close();
233 
234  trqLeftLegPort.close();
235  trqRightLegPort.close();
236  trqTorsoPort.close();
237  trqLeftArmPort.close();
238  trqRightArmPort.close();
239 
240  if (dd_descClnt)
241  {
242  dd_descClnt->close();
243  delete dd_descClnt;
244  dd_descClnt = 0;
245  }
246  if (dd_descSrv)
247  {
248  dd_descSrv->close();
249  delete dd_descSrv;
250  dd_descSrv = 0;
251  }
252 
253 
254  yInfo("Successfully terminated...bye...\n");
255  return true;
256 }
257 
258 // this should become a close
260  stopped = true;
261 
262  if (iCubLArm!=NULL) {
263  delete iCubLArm;
264  iCubLArm = NULL;
265  }
266  if (iCubRArm!=NULL) {
267  delete iCubRArm;
268  iCubRArm = NULL;
269  }
270  if (iCubHead!=NULL) {
271  delete iCubHead;
272  iCubHead = NULL;
273  }
274  if (iCubLLeg!=NULL) {
275  delete iCubLLeg;
276  iCubLLeg = NULL;
277  }
278  if (iCubRLeg!=NULL) {
279  delete iCubRLeg;
280  iCubRLeg = NULL;
281  }
282  if (iCubTorso!=NULL) {
283  delete iCubTorso;
284  iCubTorso = NULL;
285  }
286 
287  masserver.close();
288  simImu.close();
289  world_manager.clear();
290 
291  return true;
292 }
293 
294 bool SimulatorModule::read(ConnectionReader& connection){
295  //yDebug("in read...\n");
296  Bottle cmd, reply;
297  cmd.read(connection);
298  respond(cmd,reply);
299  if (connection.getWriter()!=NULL){
300  reply.write(*connection.getWriter());
301  }
302  return true;
303 }
304 
305 bool SimulatorModule::respond(const Bottle &command, Bottle &reply) {
306  string cmd = command.get(0).asString();
307  bool ok = true;
308  bool done = false;
309  if (cmd=="help") {
310  yInfo("Commands available:\n");
311  yInfo("\tleft\n");
312  yInfo("\tright\n");
313  yInfo("\twide\n");
314  yInfo("\tworld\n");
315  reply.fromString("world etc");
316  done = true;
317  } else if (cmd=="left") {
318  viewParam1 = true;
319  viewParam2 = false;
320  reply.fromString("ok");
321  done = true;
322  } else if (cmd=="right") {
323  viewParam1 = false;
324  viewParam2 = true;
325  reply.fromString("ok");
326  done = true;
327  } else if (cmd=="wide") {
328  viewParam1 = false;
329  viewParam2 = false;
330  //Simulation::start(); // sets viewpoint
331  reply.fromString("ok");
332  done = true;
333  } else if (cmd=="world"){
334  return world_manager.respond(command,reply);
335  }
336  return ok;
337 }
338 
339 yarp::dev::PolyDriver *SimulatorModule::createPart(const char *name) {
340  yDebug("Creating interface for body part %s\n", name);
341  Property options;
342  string part_file = finder.findFile(name);
343  options.fromConfigFile(part_file.c_str());
344  string part_port = options.check("name",Value(1),"what did the user select?").asString();
345  string full_name = moduleName + part_port.c_str();
346  options.put("name", full_name.c_str() );
347  options.put("joint_device", "robot_joints");
348  yarp::dev::PolyDriver *driver = new yarp::dev::PolyDriver(options);
349 
350  if (!driver->isValid()){
351  yError("Device not available. Here are the known devices:\n");
352  yError("%s", yarp::dev::Drivers::factory().toString().c_str());
353  failureToLaunch = true;
354  return NULL;
355  }
356  return driver;
357 }
358 
359 bool SimulatorModule::initSimulatorModule()
360 {
361  if (!robot_flags.valid)
362  {
363  yDebug("Robot flags are not set when creating SimulatorModule\n");
364  failureToLaunch = true;
365  return false;
366  }
367 
368  //first we try to open the robotDescriptionClient, because maybe an external server is already running
369  bool b_client = false;
370  Property clnt_opt;
371  clnt_opt.put("device", "robotDescriptionClient");
372  clnt_opt.put("local", "/icubSim/robotDescriptionClient");
373  clnt_opt.put("remote", "/robotDescription");
374  dd_descClnt = new yarp::dev::PolyDriver;
375  if (yarp::os::Network::exists("/robotDescription/rpc"))
376  {
377  if (dd_descClnt->open(clnt_opt) && dd_descClnt->isValid())
378  {
379  yInfo() << "External robotDescriptionServer was found. Connection succesful.";
380  b_client = true;
381  dd_descClnt->view(idesc);
382  }
383  else
384  {
385  yInfo() << "External robotDescriptionServer was not found. Opening a new RobotDescriptionServer.";
386  }
387  }
388  else
389  {
390  yInfo() << "External robotDescriptionServer was not found. Opening a new RobotDescriptionServer.";
391  }
392 
393  //if the previous operation failed, then retry opening first a robotDescriptionServer and then a robotDescriptionClient
394  if (b_client == false)
395  {
396  dd_descSrv = new yarp::dev::PolyDriver;
397  Property srv_opt;
398  srv_opt.put("device", "robotDescriptionServer");
399  srv_opt.put("local", "/robotDescription");
400 
401  bool b_server = false;
402  if (dd_descSrv->open(srv_opt) && dd_descSrv->isValid())
403  {
404  b_server = true;
405  }
406  else
407  {
408  //unable to open a robotDescriptionServer, this is a critical failure!
409  delete dd_descSrv;
410  dd_descSrv = 0;
411  yError() << "Unable to open robotDescriptionServer";
412  failureToLaunch = true;
413  return false;
414  }
415 
416  //robotDescriptionServer is running, so let's retry to open the robotDescriptionClient
417  if (dd_descClnt->open(clnt_opt) && dd_descClnt->isValid())
418  {
419  b_client = true;
420  dd_descClnt->view(idesc);
421  }
422  else
423  {
424  //unable to open the robotDescriptionClient, this is a critical failure!
425  delete dd_descClnt;
426  dd_descClnt = 0;
427  yError() << "Unable to open robotDescriptionClient";
428  failureToLaunch = true;
429  return false;
430  }
431  }
432 
433  Property options;
434  yarp::dev::DeviceDescription desc;
435  if (robot_flags.actLArm || robot_flags.actLHand) {
436  //start left arm device driver
437  iCubLArm = createPart("left_arm");
438  desc.device_name = moduleName+"/left_arm";
439  desc.device_type = "controlboardwrapper2";
440  if (idesc) idesc->registerDevice(desc);
441  }
442 
443  if (robot_flags.actRArm || robot_flags.actRHand) {
444  //start right arm device driver
445  iCubRArm = createPart("right_arm");
446  desc.device_name = moduleName + "/right_arm";
447  desc.device_type = "controlboardwrapper2";
448  if (idesc) idesc->registerDevice(desc);
449  }
450 
451  if (robot_flags.actHead) {
452  //start head device driver
453  iCubHead = createPart("head");
454  desc.device_name = moduleName + "/head";
455  desc.device_type = "controlboardwrapper2";
456  if (idesc) idesc->registerDevice(desc);
457  }
458 
459  if (robot_flags.actLegs) {
460  //start left leg device driver
461  iCubLLeg = createPart("left_leg");
462  desc.device_name = moduleName + "/left_leg";
463  desc.device_type = "controlboardwrapper2";
464  if (idesc) idesc->registerDevice(desc);
465  }
466 
467  if (robot_flags.actLegs) {
468  //start right leg device driver
469  iCubRLeg = createPart("right_leg");
470  desc.device_name = moduleName + "/right_leg";
471  desc.device_type = "controlboardwrapper2";
472  if (idesc) idesc->registerDevice(desc);
473  }
474  if (robot_flags.actTorso) {
475  //start torso device driver
476  iCubTorso = createPart("torso");
477  desc.device_name = moduleName + "/torso";
478  desc.device_type = "controlboardwrapper2";
479  if (idesc) idesc->registerDevice(desc);
480  }
481 
482  Property masServerConf {{"device", Value("multipleanalogsensorsserver")},
483  {"name",Value(moduleName+"/head/inertials")},
484  {"period",Value(10)}};
485  Property simImuConf {{"device",Value("simulationIMU")}};
486 
487 
488  if (!simImu.open(simImuConf)) {
489  return false;
490  }
491 
492  polyList.push(&simImu,"simImu");
493 
494  if (!masserver.open(masServerConf)){
495  return false;
496  }
497 
498  if (!masserver.view(iMWrapper))
499  {
500  yError()<<"Failed to view the IMultipleWrapper";
501  return false;
502  }
503 
504  iMWrapper->attachAll(polyList);
505  //dd_descClnt will be not used anymore.
506  dd_descClnt->close();
507  delete dd_descClnt;
508  dd_descClnt = 0;
509 
510  return true;
511 }
512 
513 void SimulatorModule::initImagePorts() {
514  Property options;
515 
516  string cameras = finder.findFile("cameras");
517  options.fromConfigFile(cameras.c_str());
518 
519  string nameExternal =
520  options.check("name_wide",
521  Value("/cam"),
522  "Name of external view camera port").asString();
523 
524  string nameLeft =
525  options.check("name_left",
526  Value("/cam/left"),
527  "Name of left camera port").asString();
528 
529  string nameRight =
530  options.check("name_right",
531  Value("/cam/right"),
532  "Name of right camera port").asString();
533 
534 #ifndef OMIT_LOGPOLAR
535  string nameLeftFov =
536  options.check("name_leftFov",
537  Value("/cam/left/fov"),
538  "Name of left camera fovea port").asString();
539  string nameRightFov =
540  options.check("name_rightFov",
541  Value("/cam/right/fov"),
542  "Name of right camera fovea port ").asString();
543 
544  string nameLeftLog =
545  options.check("name_leftLog",
546  Value("/cam/left/logpolar"),
547  "Name of left camera logpolar port").asString();
548  string nameRightLog =
549  options.check("name_rightLog",
550  Value("/cam/right/logpolar"),
551  "Name of right camera logpolar port").asString();
552 #endif
553 
554  string leftCam = moduleName + nameLeft.c_str();
555  string rightCam = moduleName + nameRight.c_str();
556  string mainCam = moduleName + nameExternal.c_str();
557 
558 #ifndef OMIT_LOGPOLAR
559  string leftFov = moduleName + nameLeftFov.c_str();
560  string rightFov = moduleName + nameRightFov.c_str();
561  string leftLog = moduleName + nameLeftLog.c_str();
562  string rightLog = moduleName + nameRightLog.c_str();
563 #endif
564 
565  portLeft.open( leftCam.c_str());
566  portRight.open( rightCam.c_str() );
567  portWide.open( mainCam.c_str() );
568 
569 #ifndef OMIT_LOGPOLAR
570  portLeftFov.open( leftFov.c_str() );
571  portRightFov.open( rightFov.c_str() );
572 
573  portLeftLog.open( leftLog.c_str() );
574  portRightLog.open( rightLog.c_str() );
575 #endif
576 }
577 
578 
580  cmdPort.setReader(*this);
581  string world = moduleName + "/world";
582 
583  string tactileLeft = moduleName + "/skin/left_hand_comp"; //Matej - changed the name to comp - this corresponds to the real iCub convention - higher values ~ higher pressure
584  string tactileRight = moduleName + "/skin/right_hand_comp";
585  string tactileLeftrpc = moduleName + "/skin/left_hand_comp/rpc:i";
586  string tactileRightrpc = moduleName + "/skin/right_hand_comp/rpc:i";
587 
588  string torqueLeftLeg = moduleName +"/joint_vsens/left_leg:i";
589  string torqueRightLeg = moduleName +"/joint_vsens/right_leg:i";
590  string torqueTorso = moduleName +"/joint_vsens/torso:i";
591  string torqueRightArm = moduleName +"/joint_vsens/left_arm:i";
592  string torqueLeftArm = moduleName +"/joint_vsens/right_arm:i";
593 
594  string inertial = moduleName + "/inertial";
595  cmdPort.open( world.c_str() );
596  tactileLeftHandPort.open( tactileLeft.c_str() );
597  tactileLeftHandPortrpc.open( tactileLeftrpc.c_str() );
598  tactileRightHandPort.open( tactileRight.c_str() );
599  tactileRightHandPortrpc.open( tactileRightrpc.c_str() );
600  inertialPort.open( inertial.c_str() );
601 
602  trqLeftLegPort.open( torqueLeftLeg.c_str() );
603  trqRightLegPort.open( torqueRightLeg.c_str() );
604  trqTorsoPort.open( torqueTorso.c_str() );
605  trqLeftArmPort.open( torqueLeftArm.c_str() );
606  trqRightArmPort.open( torqueRightArm.c_str() );
607 
608  //whole_body_skin_emul
609  string skinEventsPortString = moduleName + "/skinManager/skin_events:o";
610  skinEventsPort.open(skinEventsPortString.c_str());
611  string tactileLeftArmPortString = moduleName + "/skin/left_arm_comp";
612  tactileLeftArmPort.open(tactileLeftArmPortString.c_str());
613  string tactileRightArmPortString = moduleName + "/skin/right_arm_comp";
614  tactileRightArmPort.open(tactileRightArmPortString.c_str());
615  string tactileLeftForearmPortString = moduleName + "/skin/left_forearm_comp";
616  tactileLeftForearmPort.open(tactileLeftForearmPortString.c_str());
617  string tactileRightForearmPortString = moduleName + "/skin/right_forearm_comp";
618  tactileRightForearmPort.open(tactileRightForearmPortString.c_str());
619  string tactileTorsoPortString = moduleName + "/skin/torso_comp";
620  tactileTorsoPort.open(tactileTorsoPortString.c_str());
621 
622  if (robot_flags.actVision) {
623  initImagePorts();
624  }
625  initSimulatorModule();
626  if (failureToLaunch) return false;
627 
628  sim->init(this,&robot_config);
629 
630  w = 480;
631  h = 640;
632 
633  // the main image buffer.
634  buffer.resize( w, h);
635 
636  firstpass = true;
637 
638  return true;
639 }
640 
642 
643  Time::delay(1); // there's some clash over keyboard on Windows
644  sim->simLoop(h,w);
645  return true;
646 }
647 
648 void SimulatorModule::checkTorques()
649 {
650  bool needLeftLeg = (trqLeftLegPort.getInputCount()>0);
651  bool needRightLeg = (trqRightLegPort.getInputCount()>0);
652  bool needLeftArm = (trqLeftArmPort.getInputCount()>0);
653  bool needRightArm = (trqRightArmPort.getInputCount()>0);
654  bool needTorso = (trqTorsoPort.getInputCount()>0);
655 
656  if (needLeftLeg)
657  getTorques( trqLeftLegPort );
658  if (needRightLeg)
659  getTorques( trqRightLegPort );
660  if (needLeftArm)
661  getTorques( trqLeftArmPort );
662  if (needRightArm)
663  getTorques( trqRightArmPort );
664  if (needTorso)
665  getTorques( trqTorsoPort );
666 }
667 
668 void SimulatorModule::getTorques( yarp::os::BufferedPort<yarp::os::Bottle>& buffPort )
669 {
670  lock_guard<mutex> lck(mtx);
671  Bottle *torques = NULL;
672  torques = buffPort.read(false);
673  if ( torques !=NULL )
674  {
675  sim->getTrqData( torques->tail() );
676  }
677 }
678 
679 void SimulatorModule::displayStep(int pause) {
680  //if (sim->checkSync())
681  {
682  bool needLeft = (portLeft.getOutputCount()>0);
683  bool needRight = (portRight.getOutputCount()>0);
684  bool needWide = (portWide.getOutputCount()>0);
685 #ifndef OMIT_LOGPOLAR
686  bool needLeftFov = (portLeftFov.getOutputCount()>0);
687  bool needLeftLog = (portLeftLog.getOutputCount()>0);
688  bool needRightFov = (portRightFov.getOutputCount()>0);
689  bool needRightLog = (portRightLog.getOutputCount()>0);
690 #endif
691 
692  mtx.lock();
693  //stopping
694  bool done = stopped;
695  mtx.unlock();
696 
697 #ifndef WIN32_DYNAMIC_LINK
698  const char *order = "lrw";
699  if (viewParam1) {
700  order = "rwl";
701  }
702  if (viewParam2) {
703  order = "lwr";
704  }
705 
706  camerasStamp.update(Time::now());
707 
708  for (int i=0; i<3; i++) {
709  char ch = order[i];
710  switch (ch) {
711  case 'l':
712  if (needLeft) {
713  sim->drawView(true,false,false);
714  getImage();
715  sendImage(portLeft);
716  sim->clearBuffer();
717  }
718 #ifndef OMIT_LOGPOLAR
719  if (needLeftFov) {
720  sim->drawView(true,false,false);
721  getImage();
722  sendImageFov(portLeftFov);
723  sim->clearBuffer();
724  }
725  if (needLeftLog) {
726  sim->drawView(true,false,false);
727  getImage();
728  sendImageLog(portLeftLog);
729  sim->clearBuffer();
730  }
731 #endif
732  break;
733  case 'r':
734  if (needRight) {
735  sim->drawView(false,true,false);
736  getImage();
737  sendImage(portRight);
738  sim->clearBuffer();
739  }
740 #ifndef OMIT_LOGPOLAR
741  if (needRightFov) {
742  sim->drawView(false,true,false);
743  getImage();
744  sendImageFov(portRightFov);
745  sim->clearBuffer();
746  }
747  if (needRightLog) {
748  sim->drawView(false,true,false);
749  getImage();
750  sendImageLog(portRightLog);
751  sim->clearBuffer();
752  }
753 #endif
754  break;
755  case 'w':
756  if (needWide) {
757  sim->drawView(false,false,true);
758  getImage();
759  sendImage(portWide);
760  sim->clearBuffer();
761  }
762  break;
763  }
764  }
765 #else
766  // per-image operations can be done here
767  if (wrappedStep!=NULL) {
768  wrappedStep(pause);
769  }
770 #endif
771 
772  double now = Time::now();
773  static double first = now;
774  static double target = 0;
775  now -= first;
776  target += sloth;
777  if (target>now) {
778  Time::delay(target-now);
779  }
780  //sim->checkSync(true);
781  }
782 }
783 
784 void SimulatorModule::getImage(){
785  sim->getImage(buffer);
786 }
787 
788 void SimulatorModule::sendImage(BufferedPort<ImageOf<PixelRgb> >& port) {
789  ImageOf<PixelRgb>& normal = port.prepare();
790  normal.copy( buffer );
791  port.setEnvelope(camerasStamp);
792  port.write();
793 }
794 
795 #ifndef OMIT_LOGPOLAR
796 
797 void SimulatorModule::sendImageFov(BufferedPort<ImageOf<PixelRgb> >& portFov) {
798  static ImageOf<PixelRgb> fov;
799  ImageOf<PixelRgb>& targetFov = portFov.prepare();
800  subsampleFovea( fov, buffer );
801  targetFov.copy( fov );
802  portFov.write();
803 }
804 
805 void SimulatorModule::sendImageLog(BufferedPort<ImageOf<PixelRgb> >& portLog) {
806  static ImageOf<PixelRgb> lp;
807  lp.resize (252, 152);
808  lp.zero();
809  ImageOf<PixelRgb>& targetLog = portLog.prepare();
810  cartToLogPolar( lp , buffer );
811  targetLog.copy( lp );
812  portLog.write();
813 }
814 
815 
816 bool SimulatorModule::cartToLogPolar(ImageOf<PixelRgb>& lp, const ImageOf<PixelRgb>& cart) {
817  //
818  if (firstpass) {
819  if (!trsf.allocated())
820  trsf.allocLookupTables(C2L, lp.height(), lp.width(), cart.width(), cart.height(), 1.);
821 
822  firstpass = false;
823  }
824 
825  return trsf.cartToLogpolar(lp, cart);
826 }
827 
828 bool SimulatorModule::subsampleFovea(yarp::sig::ImageOf<yarp::sig::PixelRgb>& dst,
829  const yarp::sig::ImageOf<yarp::sig::PixelRgb>& src) {
830  //
831  dst.resize (ifovea, ifovea);
832  return iCub::logpolar::subsampleFovea(dst, src);
833 }
834 
835 #endif
SimulatorModule::shouldSendTouchTorso
virtual bool shouldSendTouchTorso()
Definition: SimulatorModule.cpp:172
SimulatorModule::sendSkinEvents
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)
Definition: SimulatorModule.cpp:108
Simulation::init
virtual void init(RobotStreamer *streamer, RobotConfig *config)=0
Initialization.
iCub::skinDynLib::skinContactList
Definition: skinContactList.h:44
RobotFlags::actLHand
bool actLHand
Definition: RobotConfig.h:31
SimulatorModule::interruptModule
bool interruptModule()
Definition: SimulatorModule.cpp:259
SimulatorModule::shouldSendTouchLeftForearm
virtual bool shouldSendTouchLeftForearm()
Definition: SimulatorModule.cpp:157
SimulatorModule::shouldSendTouchLeftHand
virtual bool shouldSendTouchLeftHand()
Definition: SimulatorModule.cpp:98
SimulatorModule::sendTouchTorso
virtual void sendTouchTorso(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:165
SimulatorModule::shouldSendTouchLeftArm
virtual bool shouldSendTouchLeftArm()
Definition: SimulatorModule.cpp:135
viewParam2
bool viewParam2
Definition: SimulatorModule.cpp:49
SimulatorModule::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: SimulatorModule.cpp:305
RobotConfig
Definition: RobotConfig.h:55
RobotFlags::actHead
bool actHead
Definition: RobotConfig.h:31
SimulatorModule::SimulatorModule
SimulatorModule(WorldManager &world, RobotConfig &config, Simulation *sim)
Definition: SimulatorModule.cpp:56
cmd
cmd
Definition: dataTypes.h:30
WorldManager::clear
virtual void clear()
Definition: WorldManager.h:35
Simulation::getImage
virtual bool getImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &img)=0
RobotFlags::valid
bool valid
Definition: RobotConfig.h:30
Simulation::clearBuffer
virtual void clearBuffer()=0
Signal that we're done with a view.
RobotFlags::actRArm
bool actRArm
Definition: RobotConfig.h:31
baseHeight
const int baseHeight
Definition: SimulatorModule.cpp:54
SimulatorModule::sendVision
virtual void sendVision()
Definition: SimulatorModule.cpp:188
WorldManager
Definition: WorldManager.h:28
Simulation::simLoop
virtual void simLoop(int h, int w)=0
Run the simulation.
SimulatorModule::shouldSendSkinEvents
virtual bool shouldSendSkinEvents()
Definition: SimulatorModule.cpp:117
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
RobotFlags::actTorso
bool actTorso
Definition: RobotConfig.h:31
SimulatorModule::sendTouchRightArm
virtual void sendTouchRightArm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:128
RobotFlags::actVision
bool actVision
Definition: RobotConfig.h:31
SimulatorModule::shouldSendInertial
virtual bool shouldSendInertial()
Definition: SimulatorModule.cpp:184
noCam
bool noCam
Definition: SimulatorModule.cpp:50
viewParam1
bool viewParam1
Definition: SimulatorModule.cpp:49
WorldManager::respond
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: FakeWorldManager.cpp:13
SimulatorModule::shouldSendTouchRightHand
virtual bool shouldSendTouchRightHand()
Definition: SimulatorModule.cpp:102
SimulatorModule::sendInertial
virtual void sendInertial(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:177
state::ok
@ ok
SimulatorModule.h
SimulatorModule::sendTouchRightHand
virtual void sendTouchRightHand(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:91
Simulation::drawView
virtual void drawView(bool left, bool right, bool wide)=0
Render the requested view.
SimulatorModule::shouldSendTouchRightForearm
virtual bool shouldSendTouchRightForearm()
Definition: SimulatorModule.cpp:161
Simulation
Definition: Simulation.h:28
RobotFlags::actLegs
bool actLegs
Definition: RobotConfig.h:31
done
bool done
Definition: main.cpp:42
SimulatorModule::shouldSendTouchRightArm
virtual bool shouldSendTouchRightArm()
Definition: SimulatorModule.cpp:139
SimulatorModule::read
bool read(yarp::os::ConnectionReader &connection)
Definition: SimulatorModule.cpp:294
Simulation::getTrqData
virtual bool getTrqData(yarp::os::Bottle data)=0
RobotFlags::actLArm
bool actLArm
Definition: RobotConfig.h:31
RobotFlags::actRHand
bool actRHand
Definition: RobotConfig.h:31
SimulatorModule::sendTouchRightForearm
virtual void sendTouchRightForearm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:150
robot_config
static RobotConfig * robot_config
Definition: iCub_Sim.cpp:84
SimulatorModule::closeModule
bool closeModule()
Definition: SimulatorModule.cpp:192
baseWidth
const int baseWidth
Definition: SimulatorModule.cpp:53
SimulatorModule::open
bool open()
Definition: SimulatorModule.cpp:579
SimulatorModule::sendTouchLeftArm
virtual void sendTouchLeftArm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:121
ifovea
const int ifovea
Definition: SimulatorModule.cpp:52
SimulatorModule::sendTouchLeftHand
virtual void sendTouchLeftHand(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:84
SimulatorModule::runModule
bool runModule()
Definition: SimulatorModule.cpp:641
SimulatorModule::sendTouchLeftForearm
virtual void sendTouchLeftForearm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:143