virtual void threadRelease() { /* Stop and close ports */ cportL->interrupt(); cportR->interrupt(); susPort->interrupt(); cportL->close(); cportR->close(); susPort->close(); delete cportL; delete cportR; delete susPort; /* stop motor interfaces and close */ gaze->stopControl(); clientGazeCtrl.close(); carm->stopControl(); clientArmCart.close(); robotTorso.close(); robotHead.close(); robotArm.close(); }
bool DeviceGroup::open(const char *key, PolyDriver& poly, yarp::os::Searchable& config, const char *comment) { Value *name; if (config.check(key,name,comment)) { if (name->isString()) { // maybe user isn't doing nested configuration yarp::os::Property p; p.setMonitor(config.getMonitor(), name->toString().c_str()); // pass on any monitoring p.fromString(config.toString()); p.put("device",name->toString()); p.unput("subdevice"); p.unput("wrapped"); poly.open(p); } else { Bottle subdevice = config.findGroup(key).tail(); poly.open(subdevice); } if (!poly.isValid()) { printf("cannot make <%s>\n", name->toString().c_str()); return false; } } else { printf("\"--%s <name>\" not set\n", key); return false; } return true; }
bool close() { if (driver.isValid()) driver.close(); return true; }
bool threadInit() { string name=rf.find("name").asString().c_str(); double period=rf.find("period").asDouble(); setRate(int(1000.0*period)); // open a client interface to connect to the gaze server // we suppose that: // 1 - the iCub simulator (icubSim) is running // 2 - the gaze server iKinGazeCtrl is running and // launched with the following options: // --robot icubSim --context cameraCalibration/conf --config icubSimEyes.ini Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local",("/"+name+"/gaze").c_str()); if (!clientGaze.open(optGaze)) return false; // open the view clientGaze.view(igaze); // latch the controller context in order to preserve // it after closing the module // the context contains the tracking mode, the neck limits and so on. igaze->storeContext(&startup_context_id); // set trajectory time: igaze->setNeckTrajTime(0.8); igaze->setEyesTrajTime(0.4); port.open(("/"+name+"/target:i").c_str()); return true; }
bool open(Searchable& p) { bool dev = true; if (p.check("nodevice")) { dev = false; } if (dev) { poly.open(p); if (!poly.isValid()) { printf("cannot open driver\n"); return false; } if (!p.check("mute")) { // Make sure we can write sound poly.view(put); if (put==NULL) { printf("cannot open interface\n"); return false; } } } port.setStrict(true); if (!port.open(p.check("name",Value("/yarphear")).asString())) { printf("Communication problem\n"); return false; } if (p.check("remote")) { Network::connect(p.check("remote",Value("/remote")).asString(), port.getName()); } return true; }
bool threadInit() { //initialize here variables printf("ControlThread:starting\n"); Property options; options.put("device", "remote_controlboard"); options.put("local", "/local/head"); //substitute icubSim with icub for use with the real robot options.put("remote", "/icubSim/head"); dd.open(options); dd.view(iencs); dd.view(ivel); if ( (!iencs) || (!ivel) ) return false; int joints; iencs->getAxes(&joints); encoders.resize(joints); commands.resize(joints); commands=10000; ivel->setRefAccelerations(commands.data()); count=0; return true; }
bool add(const char *name, yarp::os::Searchable& config) { //printf("ADDING %s\n", config.toString().c_str()); PolyDriver *pd = new PolyDriver(); YARP_ASSERT (pd!=NULL); bool result = pd->open(config); if (!result) { delete pd; return false; } drivers.push_back(pd); names.push_back(ConstString(name)); IService *service = NULL; pd->view(service); bool backgrounded = true; if (service!=NULL) { backgrounded = service->startService(); if (backgrounded) { // we don't need to poll this, so forget about the // service interface printf("group: service backgrounded\n"); service = NULL; } } needDrive.push_back(!backgrounded); needDriveSummary = needDriveSummary || (!backgrounded); Drivers::factory().add(new DriverLinkCreator(name,*pd)); return true; }
virtual bool threadInit() { string name=rf.check("name",Value("faceTracker")).asString().c_str(); string robot=rf.check("robot",Value("icub")).asString().c_str(); int period=rf.check("period",Value(50)).asInt(); eye=rf.check("eye",Value("left")).asString().c_str(); arm=rf.check("arm",Value("right")).asString().c_str(); eyeDist=rf.check("eyeDist",Value(1.0)).asDouble(); holdoff=rf.check("holdoff",Value(3.0)).asDouble(); Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local",("/"+name+"/gaze_client").c_str()); if (!clientGaze.open(optGaze)) return false; clientGaze.view(igaze); igaze->storeContext(&startup_gazeContext_id); igaze->blockNeckRoll(0.0); if (arm!="none") { Property optArm("(device cartesiancontrollerclient)"); optArm.put("remote",("/"+robot+"/cartesianController/"+arm+"_arm").c_str()); optArm.put("local",("/"+name+"/arm_client").c_str()); if (!clientArm.open(optArm)) return false; clientArm.view(iarm); iarm->storeContext(&startup_armContext_id); } portImgIn.open(("/"+name+"/img:i").c_str()); portImgOut.open(("/"+name+"/img:o").c_str()); portTopDown.open(("/"+name+"/topdown:i").c_str()); portSetFace.open(("/"+name+"/setFace:rpc").c_str()); if (!fd.init(rf.findFile("descriptor").c_str())) { fprintf(stdout,"Cannot load descriptor!\n"); return false; } Rand::init(); resetState(); armCmdState=0; queuedFaceExprFlag=false; setRate(period); cvSetNumThreads(1); t0=Time::now(); return true; }
bool configure(ResourceFinder &rf) { Property options; options.put("device","fakeyServer"); options.put("local","/fake_robot/fake_part"); driver.open(options); return driver.isValid(); }
virtual bool threadInit() { // open a client interface to connect to the gaze server // we suppose that: // 1 - the iCub simulator is running; // 2 - the gaze server iKinGazeCtrl is running and // launched with the following options: "--from configSim.ini" Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local","/gaze_client"); if (!clientGaze.open(optGaze)) return false; // open the view clientGaze.view(igaze); // latch the controller context in order to preserve // it after closing the module // the context contains the tracking mode, the neck limits and so on. igaze->storeContext(&startup_context_id); // set trajectory time: igaze->setNeckTrajTime(0.8); igaze->setEyesTrajTime(0.4); // put the gaze in tracking mode, so that // when the torso moves, the gaze controller // will compensate for it igaze->setTrackingMode(true); // print out some info about the controller Bottle info; igaze->getInfo(info); fprintf(stdout,"info = %s\n",info.toString().c_str()); Property optTorso("(device remote_controlboard)"); optTorso.put("remote","/icubSim/torso"); optTorso.put("local","/torso_client"); if (!clientTorso.open(optTorso)) return false; // open the view clientTorso.view(ienc); clientTorso.view(ipos); ipos->setRefSpeed(0,10.0); fp.resize(3); state=STATE_TRACK; t=t0=t1=t2=t3=t4=Time::now(); return true; }
virtual bool threadInit() { // open a client interface to connect to the gaze server // we suppose that: // 1 - the iCub simulator (icubSim) is running // 2 - the gaze server iKinGazeCtrl is running and // launched with --robot icubSim option Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local","/gaze_client"); clientGaze=new PolyDriver; if (!clientGaze->open(optGaze)) { delete clientGaze; return false; } // open the view clientGaze->view(igaze); // set trajectory time: // we'll go like hell since we're using the simulator :) igaze->setNeckTrajTime(0.4); igaze->setEyesTrajTime(0.1); // put the gaze in tracking mode, so that // when the torso moves, the gaze controller // will compensate for it /*pramod modified starts igaze->setTrackingMode(true); Property optTorso("(device remote_controlboard)"); optTorso.put("remote","/icubSim/torso"); optTorso.put("local","/torso_client"); clientTorso=new PolyDriver; if (!clientTorso->open(optTorso)) { delete clientTorso; return false; } // open the view clientTorso->view(ienc); clientTorso->view(ipos); ipos->setRefSpeed(0,10.0); pramod modified ends */ fp.resize(3); state=STATE_TRACK; t=t0=t1=t2=t3=t4=Time::now(); return true; }
virtual void threadRelease() { // we require an immediate stop // before closing the client for safety reason igaze->stopControl(); // it's a good rule to restore the controller // context as it was before opening the module igaze->restoreContext(startup_context_id); clientGaze.close(); clientTorso.close(); }
bool close() { igaze->restoreContext(startup_context_gaze); drvGaze.close(); iarm->restoreContext(startup_context_arm); drvArm.close(); imgLPortIn.close(); imgRPortIn.close(); imgLPortOut.close(); imgRPortOut.close(); rpcPort.close(); return true; }
//******************************************** bool close() { yInfo("[demoAvoidance] Closing module.."); dataPort.close(); if (useLeftArm) { if (driverCartL.isValid()) { data["left"].iarm->stopControl(); data["left"].iarm->restoreContext(contextL); driverCartL.close(); } if (driverJointL.isValid()) { IInteractionMode *imode; driverJointL.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointL.close(); } } if (useRightArm) { if (driverCartR.isValid()) { data["right"].iarm->stopControl(); data["right"].iarm->restoreContext(contextR); driverCartR.close(); } if (driverJointR.isValid()) { IInteractionMode *imode; driverJointR.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointR.close(); } } return true; }
Gazer(PolyDriver &driver, ActionPrimitives *action, const Vector &offset) : RateThread(50) { driver.view(this->igaze); this->action=action; this->offset=offset; }
int main(int argc, char *argv[]) { Network yarp; // Initialize yarp framework // use YARP to create and configure an instance of fakeDepthCamera Property config; config.put("device", "fakeDepthCamera"); // device producing (fake) data config.put("mode", "ball"); // fake data type to be produced PolyDriver dd; dd.open(config); if (!dd.isValid()) { yError("Failed to create and configure a the fake device\n"); return 1; } yarp::dev::IRGBDSensor *RGBDInterface; // interface we want to use if (!dd.view(RGBDInterface)) // grab wanted device interface { yError("Failed to get RGBDInterface device interface\n"); return 1; } // Let's use the interface to get info from device int rgbImageHeight = RGBDInterface->getRgbHeight(); int rgbImageWidth = RGBDInterface->getRgbWidth(); int depthImageHeight = RGBDInterface->getDepthHeight(); int depthImageWidth = RGBDInterface->getDepthWidth(); FlexImage rgbImage; ImageOf<PixelFloat> depthImage; bool gotImage = RGBDInterface->getImages(rgbImage, depthImage); if(gotImage) yInfo("Succesfully retieved an image"); else yError("Failed retieving images"); yarp::os::Property intrinsic; RGBDInterface->getRgbIntrinsicParam(intrinsic); yInfo("RGB intrinsic parameters: \n%s", intrinsic.toString().c_str()); dd.close(); return 0; }
bool connect() { drv=new PolyDriver(drvOptions); if (drv->isValid()) connected=drv->view(pos)&&drv->view(enc); else connected=false; if (!connected) { delete drv; drv=0; } return connected; }
void testBasic() { report(0,"a very basic driver instantiation test"); PolyDriver dd; Property p; p.put("device","test_grabber"); bool result; result = dd.open(p); checkTrue(result,"open reported successful"); IFrameGrabberImage *grabber = NULL; result = dd.view(grabber); checkTrue(result,"interface reported"); ImageOf<PixelRgb> img; grabber->getImage(img); checkTrue(img.width()>0,"interface seems functional"); result = dd.close(); checkTrue(result,"close reported successful"); }
bool OnlineStictionEstimator::configure(PolyDriver &driver, const Property &options) { if (driver.isValid() && options.check("joint")) { bool ok=true; ok&=driver.view(imod); ok&=driver.view(ilim); ok&=driver.view(ienc); ok&=driver.view(ipid); ok&=driver.view(iolc); if (!ok) return false; joint=options.find("joint").asInt(); setRate((int)(1000.0*options.check("Ts",Value(0.01)).asDouble())); T=options.check("T",Value(2.0)).asDouble(); Kp=options.check("Kp",Value(10.0)).asDouble(); Ki=options.check("Ki",Value(250.0)).asDouble(); Kd=options.check("Kd",Value(15.0)).asDouble(); vel_thres=fabs(options.check("vel_thres",Value(5.0)).asDouble()); e_thres=fabs(options.check("e_thres",Value(1.0)).asDouble()); gamma.resize(2,1.0); if (Bottle *pB=options.find("gamma").asList()) { size_t len=std::min(gamma.length(),(size_t)pB->size()); for (size_t i=0; i<len; i++) gamma[i]=pB->get(i).asDouble(); } stiction.resize(2,0.0); if (Bottle *pB=options.find("stiction").asList()) { size_t len=std::min(stiction.length(),(size_t)pB->size()); for (size_t i=0; i<len; i++) stiction[i]=pB->get(i).asDouble(); } return configured=true; } else return false; }
// helper method for "yarpdev" body static void toDox(PolyDriver& dd, FILE *os) { fprintf(os,"===============================================================\n"); fprintf(os,"== Options checked by device:\n== \n"); Bottle order = dd.getOptions(); for (size_t i=0; i<order.size(); i++) { std::string name = order.get(i).toString(); if (name=="wrapped"||(name.find(".wrapped")!=std::string::npos)) { continue; } std::string desc = dd.getComment(name.c_str()); Value def = dd.getDefaultValue(name.c_str()); Value actual = dd.getValue(name.c_str()); std::string out = ""; out += name; if (!actual.isNull()) { if (actual.toString()!="") { out += "="; if (actual.toString().length()<40) { out += actual.toString().c_str(); } else { out += "(value too long)"; } } } if (!def.isNull()) { if (def.toString()!="") { out += " ["; if (def.toString().length()<40) { out += def.toString().c_str(); } else { out += "(value too long)"; } out += "]"; } } if (desc!="") { out += "\n "; out += desc.c_str(); } fprintf(os,"%s\n", out.c_str()); } fprintf(os,"==\n"); fprintf(os,"===============================================================\n"); }
void idlingCoupledJoints(const int i, const bool sw) { IControlMode2 *imod; driver->view(imod); PidData &pid=pids[i]; for (size_t j=0; j<pid.idling_joints.size(); j++) imod->setControlMode(pid.idling_joints[j], sw?VOCAB_CM_IDLE:VOCAB_CM_POSITION); }
bool close() { driver.close(); Property options; model->toProperty(options); fprintf(stdout,"model options: %s\n",options.toString().c_str()); return true; }
void testMotor(PolyDriver& driver) { IPositionControl *pos; if (driver.view(pos)) { int ct = 0; pos->getAxes(&ct); printf(" number of axes is: %d\n", ct); } else { printf(" could not find IPositionControl interface\n"); } }
void threadRelease() { printf("ControlThread:stopping the robot\n"); ivel->stop(); dd.close(); printf("Done, goodbye from ControlThread\n"); }
bool configure(ResourceFinder &rf) { string mode=rf.check("mode",Value("physical")).asString().c_str(); Property option; if (mode=="physical") option.put("device","geomagicdriver"); else { option.put("device","hapticdeviceclient"); option.put("remote","/hapticdevice"); option.put("local","/client"); } if (!driver.open(option)) return false; driver.view(igeo); return true; }
//******************************************** bool close() { dataPort.close(); if (driverCartL.isValid()) { data["left"].iarm->stopControl(); data["left"].iarm->restoreContext(contextL); driverCartL.close(); } if (driverCartR.isValid()) { data["right"].iarm->stopControl(); data["right"].iarm->restoreContext(contextR); driverCartR.close(); } if (driverJointL.isValid()) { IInteractionMode *imode; driverJointL.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointL.close(); } if (driverJointR.isValid()) { IInteractionMode *imode; driverJointR.view(imode); for (int j=0; j<5; j++) imode->setInteractionMode(j,VOCAB_IM_STIFF); driverJointR.close(); } return true; }
/** Lorenzo Natale, Dec 2007. Test remotization of calibrate * functions. Useful for general tests on device remotization * (see fakebot for example). */ int main(int argc, const char **argv) { Network yarp; IControlCalibration2 *ical; Property p; p.put("device", "remote_controlboard"); p.put("local", "/motortest"); //prefix for local names p.put("remote", "/controlboard"); //prefix for remote names PolyDriver device; device.open(p); device.view(ical); //ical->calibrate(); ical->calibrate2(1,1000,1.1,1.1,1.1); ical->done(2); ical->park(); device.close(); return 0; }
virtual bool close() { yInfo("Closing module [%s]\n", partName); vc->stop(); vc->halt(); delete vc; yInfo("Thead [%s] stopped\n", partName); driver.close(); input_port.close(); yInfo("Module [%s] closed\n", partName); return true; }
virtual bool close() { yInfo("Closing module [%s]\n", partName); pThread->stop(); pThread->halt(); delete pThread; yInfo("Thread [%s] stopped\n", partName); driver.close(); rpc_port.close(); yInfo("Module [%s] closed\n", partName); return true; }
static void toDox(PolyDriver& dd, FILE *os) { fprintf(os, "<table>\n"); fprintf(os, "<tr><td>PROPERTY</td><td>DESCRIPTION</td><td>DEFAULT</td></tr>\n"); Bottle order = dd.getOptions(); for (int i=0; i<order.size(); i++) { String name = order.get(i).toString().c_str(); if (name=="wrapped"||name.substr(0,10)=="subdevice.") { continue; } ConstString desc = dd.getComment(name.c_str()); ConstString def = dd.getDefaultValue(name.c_str()).toString(); String out = ""; out += "<tr><td>"; out += name.c_str(); out += "</td><td>"; out += desc.c_str(); out += "</td><td>"; out += def.c_str(); out += "</td></tr>"; fprintf(os,"%s\n",out.c_str()); } fprintf(os, "</table>\n"); }