iCub-main
SimulatorModule.h
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 
21 
22 #ifndef ICUBSIMULATION_SIMULATORMODULE_INC
23 #define ICUBSIMULATION_SIMULATORMODULE_INC
24 
25 #include <yarp/os/Bottle.h>
26 #include <yarp/os/Stamp.h>
27 #include <yarp/sig/Image.h>
28 #include <yarp/dev/PolyDriver.h>
29 #include <yarp/dev/IRobotDescription.h>
30 #include <yarp/dev/IMultipleWrapper.h>
31 
32 #include "RobotStreamer.h"
33 #include "RobotConfig.h"
34 #include "WorldManager.h"
35 #include "Simulation.h"
36 
37 #include <mutex>
38 #include <string>
39 
40 #ifndef OMIT_LOGPOLAR
41 // wouldn't be better to have this in conditional compilation?
42 #include <iCub/logpolar/RC_DIST_FB_logpolar_mapper.h>
43 #endif
45 
46 class SimulatorModule : public yarp::os::PortReader, public RobotStreamer
47 {
48 public:
49  SimulatorModule(WorldManager& world, RobotConfig& config, Simulation *sim);
50 
51  bool open();
52  bool runModule();
53 
54  bool closeModule();
55  bool interruptModule();
56 
57  bool read(yarp::os::ConnectionReader& connection);
58 
59  bool respond(const yarp::os::Bottle &command,
60  yarp::os::Bottle &reply);
61 
62  // RobotStreamer interface
63 
64  virtual void sendVision();
65 
66  virtual void sendTouchLeftHand(yarp::os::Bottle& report);
67  virtual void sendTouchRightHand(yarp::os::Bottle& report);
68 
69  virtual bool shouldSendTouchLeftHand();
70  virtual bool shouldSendTouchRightHand();
71 
72  virtual void sendInertial(yarp::os::Bottle& report);
73  virtual bool shouldSendInertial();
74 
75  // whole_body_skin_emul
76  virtual void sendSkinEvents(iCub::skinDynLib::skinContactList& skinContactListReport);
77  virtual bool shouldSendSkinEvents();
78 
79  virtual void sendTouchLeftArm(yarp::os::Bottle& report);
80  virtual void sendTouchRightArm(yarp::os::Bottle& report);
81  virtual bool shouldSendTouchLeftArm();
82  virtual bool shouldSendTouchRightArm();
83 
84  virtual void sendTouchLeftForearm(yarp::os::Bottle& report);
85  virtual void sendTouchRightForearm(yarp::os::Bottle& report);
86  virtual bool shouldSendTouchLeftForearm();
87  virtual bool shouldSendTouchRightForearm();
88 
89  virtual void sendTouchTorso(yarp::os::Bottle& report);
90  virtual bool shouldSendTouchTorso();
91 
92 private:
93 
94 #ifndef OMIT_LOGPOLAR
95  // wrapper to logpolarTransform, taking into account initialization
96  bool cartToLogPolar(yarp::sig::ImageOf<yarp::sig::PixelRgb> &lp,
97  const yarp::sig::ImageOf<yarp::sig::PixelRgb> &cart);
98  bool subsampleFovea(yarp::sig::ImageOf<yarp::sig::PixelRgb>& dst,
99  const yarp::sig::ImageOf<yarp::sig::PixelRgb>& src);
100 
101  void sendImageFov(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> >& port);
102  void sendImageLog(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> >& port);
103 #endif
104 
105  void displayStep(int pause);
106 
107  virtual void checkTorques();
108  void getTorques( yarp::os::BufferedPort<yarp::os::Bottle>& Port );
109 
110  void getImage();
111  void sendImage(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> >& port);
112 
113  std::string moduleName;
114  yarp::dev::IRobotDescription* idesc;
115  yarp::dev::PolyDriver *dd_descSrv;
116  yarp::dev::PolyDriver *dd_descClnt;
117 
118 #ifndef OMIT_LOGPOLAR
119  iCub::logpolar::logpolarTransform trsf;
120 #endif
121  bool firstpass;
122 
123  yarp::os::Stamp camerasStamp;
124  yarp::os::Stamp generalStamp;
125  yarp::sig::ImageOf<yarp::sig::PixelRgb> buffer;
126 
127  yarp::dev::PolyDriver *iCubLArm, *iCubRArm, *iCubHead, *iCubLLeg ,*iCubRLeg, *iCubTorso;
128  yarp::dev::PolyDriver masserver, simImu;;
129  yarp::dev::PolyDriverList polyList;
130  yarp::dev::IMultipleWrapper* iMWrapper{nullptr};
131  yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > portLeft, portRight, portWide;
132 
133 #ifndef OMIT_LOGPOLAR
134  yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > portLeftFov, portLeftLog, portRightFov, portRightLog;
135 #endif
136  yarp::os::Port cmdPort;
137  yarp::os::BufferedPort<yarp::os::Bottle> tactileLeftHandPort, tactileRightHandPort, inertialPort; //Matej - renamed tactileLeftPort to tactileLeftHandPort, same for right, deleted tactilePort
138  yarp::os::BufferedPort<yarp::os::Bottle> trqLeftLegPort, trqRightLegPort, trqLeftArmPort, trqRightArmPort, trqTorsoPort;
139  yarp::os::Port tactileLeftHandPortrpc, tactileRightHandPortrpc;
140  //whole_body_skin_emul
141  yarp::os::BufferedPort<iCub::skinDynLib::skinContactList> skinEventsPort;
142  yarp::os::BufferedPort<yarp::os::Bottle> tactileLeftArmPort, tactileRightArmPort, tactileLeftForearmPort, tactileRightForearmPort, tactileTorsoPort;
143 
144  int _argc;
145  char **_argv;
146  int w, h;
147  bool stopped;
148  bool extractImages;
149  void (*wrappedStep) (int pause);
150  std::mutex mtx;
151  double sloth;
152 
153  bool initSimulatorModule();
154  void initImagePorts();
155  void initIcubParts();
156 
157  yarp::dev::PolyDriver *createPart(const char *name);
158 
159  WorldManager& world_manager;
160  RobotConfig& robot_config;
161  RobotFlags& robot_flags;
162  yarp::os::ResourceFinder& finder;
163  Simulation *sim;
164  bool failureToLaunch;
165 };
166 
167 #endif
SimulatorModule::shouldSendTouchTorso
virtual bool shouldSendTouchTorso()
Definition: SimulatorModule.cpp:172
SimulatorModule::sendSkinEvents
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)
Definition: SimulatorModule.cpp:108
iCub::skinDynLib::skinContactList
Definition: skinContactList.h:44
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
Simulation.h
SimulatorModule::respond
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: SimulatorModule.cpp:305
RobotConfig
Definition: RobotConfig.h:55
SimulatorModule::SimulatorModule
SimulatorModule(WorldManager &world, RobotConfig &config, Simulation *sim)
Definition: SimulatorModule.cpp:56
SimulatorModule::sendVision
virtual void sendVision()
Definition: SimulatorModule.cpp:188
WorldManager
Definition: WorldManager.h:28
SimulatorModule::shouldSendSkinEvents
virtual bool shouldSendSkinEvents()
Definition: SimulatorModule.cpp:117
WorldManager.h
RobotStreamer
Definition: RobotStreamer.h:29
SimulatorModule::sendTouchRightArm
virtual void sendTouchRightArm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:128
SimulatorModule::shouldSendInertial
virtual bool shouldSendInertial()
Definition: SimulatorModule.cpp:184
SimulatorModule::shouldSendTouchRightHand
virtual bool shouldSendTouchRightHand()
Definition: SimulatorModule.cpp:102
SimulatorModule::sendInertial
virtual void sendInertial(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:177
RobotStreamer.h
skinContactList.h
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
SimulatorModule::sendTouchRightHand
virtual void sendTouchRightHand(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:91
SimulatorModule::shouldSendTouchRightForearm
virtual bool shouldSendTouchRightForearm()
Definition: SimulatorModule.cpp:161
RobotConfig.h
Simulation
Definition: Simulation.h:28
SimulatorModule
Definition: SimulatorModule.h:46
SimulatorModule::shouldSendTouchRightArm
virtual bool shouldSendTouchRightArm()
Definition: SimulatorModule.cpp:139
SimulatorModule::read
bool read(yarp::os::ConnectionReader &connection)
Definition: SimulatorModule.cpp:294
SimulatorModule::sendTouchRightForearm
virtual void sendTouchRightForearm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:150
RobotFlags
Definition: RobotConfig.h:28
SimulatorModule::closeModule
bool closeModule()
Definition: SimulatorModule.cpp:192
SimulatorModule::open
bool open()
Definition: SimulatorModule.cpp:579
SimulatorModule::sendTouchLeftArm
virtual void sendTouchLeftArm(yarp::os::Bottle &report)
Definition: SimulatorModule.cpp:121
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