22 #ifndef ICUBSIMULATION_SIMULATORMODULE_INC
23 #define ICUBSIMULATION_SIMULATORMODULE_INC
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>
42 #include <iCub/logpolar/RC_DIST_FB_logpolar_mapper.h>
57 bool read(yarp::os::ConnectionReader& connection);
59 bool respond(
const yarp::os::Bottle &command,
60 yarp::os::Bottle &reply);
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);
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);
105 void displayStep(
int pause);
107 virtual void checkTorques();
108 void getTorques( yarp::os::BufferedPort<yarp::os::Bottle>& Port );
111 void sendImage(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> >& port);
113 std::string moduleName;
114 yarp::dev::IRobotDescription* idesc;
115 yarp::dev::PolyDriver *dd_descSrv;
116 yarp::dev::PolyDriver *dd_descClnt;
118 #ifndef OMIT_LOGPOLAR
119 iCub::logpolar::logpolarTransform trsf;
123 yarp::os::Stamp camerasStamp;
124 yarp::os::Stamp generalStamp;
125 yarp::sig::ImageOf<yarp::sig::PixelRgb> buffer;
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;
133 #ifndef OMIT_LOGPOLAR
134 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > portLeftFov, portLeftLog, portRightFov, portRightLog;
136 yarp::os::Port cmdPort;
137 yarp::os::BufferedPort<yarp::os::Bottle> tactileLeftHandPort, tactileRightHandPort, inertialPort;
138 yarp::os::BufferedPort<yarp::os::Bottle> trqLeftLegPort, trqRightLegPort, trqLeftArmPort, trqRightArmPort, trqTorsoPort;
139 yarp::os::Port tactileLeftHandPortrpc, tactileRightHandPortrpc;
141 yarp::os::BufferedPort<iCub::skinDynLib::skinContactList> skinEventsPort;
142 yarp::os::BufferedPort<yarp::os::Bottle> tactileLeftArmPort, tactileRightArmPort, tactileLeftForearmPort, tactileRightForearmPort, tactileTorsoPort;
149 void (*wrappedStep) (
int pause);
153 bool initSimulatorModule();
154 void initImagePorts();
155 void initIcubParts();
157 yarp::dev::PolyDriver *createPart(
const char *name);
162 yarp::os::ResourceFinder& finder;
164 bool failureToLaunch;
virtual bool shouldSendTouchRightHand()
virtual void sendTouchRightArm(yarp::os::Bottle &report)
virtual void sendTouchLeftArm(yarp::os::Bottle &report)
virtual bool shouldSendTouchLeftArm()
virtual void sendTouchTorso(yarp::os::Bottle &report)
virtual bool shouldSendTouchLeftHand()
virtual void sendSkinEvents(iCub::skinDynLib::skinContactList &skinContactListReport)
virtual void sendTouchLeftForearm(yarp::os::Bottle &report)
virtual void sendTouchLeftHand(yarp::os::Bottle &report)
SimulatorModule(WorldManager &world, RobotConfig &config, Simulation *sim)
virtual void sendTouchRightForearm(yarp::os::Bottle &report)
bool read(yarp::os::ConnectionReader &connection)
virtual bool shouldSendTouchTorso()
virtual bool shouldSendTouchLeftForearm()
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
virtual bool shouldSendTouchRightForearm()
virtual void sendVision()
virtual bool shouldSendInertial()
virtual bool shouldSendTouchRightArm()
virtual void sendTouchRightHand(yarp::os::Bottle &report)
virtual void sendInertial(yarp::os::Bottle &report)
virtual bool shouldSendSkinEvents()