void SamIter(void) { bool thereIsObservation,hardError; CObservation2DRangeScan myscan; laser.doProcessSimple( thereIsObservation, myscan, hardError ); Bottle &Send = myfirst.prepare(); Send.clear(); if(thereIsObservation ) { //scan only 512 points for 180 deg, ignore the first,last 85 points for(int x=0;x<myscan.scan.size()-85;x++) { //if(myscan.scan[x]<mindistance){myscan.scan[x]=0;} if(x>84) { Send.addDouble(myscan.scan[x]); if(myscan.scan[x]==1.0) std::cout << "obst sensed at " << x <<std::endl; //if(myscan.scan[x]<mindistance){myscan.scan[x]=0;} } //std::cout << "Scan size Vlaser" << myscan.scan.size() <<std::endl; } myfirst.writeStrict(); } //std::cout<< " laser size " <<Send.size() <<std::endl; }
bool updateModule() { LockGuard lg(mutex); double dumpTime=Time::now(); Bottle &bDump=dumpPort.prepare(); bDump.clear(); bDump.addString(actionTag); bDump.addString(objectTag); opc.checkout(); // agent body + position agentName = getPartnerName(); if (Entity *e=opc.getEntity(agentName)) { if (Agent *agent=dynamic_cast<Agent*>(e)) { bDump.addList()=agent->m_body.asBottle(); Bottle &bAgent=bDump.addList(); bAgent.addString(agent->name()); bAgent.addDouble(agent->m_ego_position[0]); bAgent.addDouble(agent->m_ego_position[1]); bAgent.addDouble(agent->m_ego_position[2]); bAgent.addInt(agent->m_present==1.0?1:0); } } // objects position list<Entity*> lEntity=opc.EntitiesCache(); for (list<Entity*>::iterator itEnt=lEntity.begin(); itEnt!=lEntity.end(); itEnt++) { string entityName=(*itEnt)->name(); string entityType=(*itEnt)->entity_type(); if (entityType==EFAA_OPC_ENTITY_OBJECT) { if (Object *object=dynamic_cast<Object*>(*itEnt)) { Bottle &bObject=bDump.addList(); bObject.addString(object->name()); bObject.addDouble(object->m_ego_position[0]); bObject.addDouble(object->m_ego_position[1]); bObject.addDouble(object->m_ego_position[2]); bObject.addInt(object->m_present==1.0?1:0); } } } bDump.addInt(gate); dumpStamp.update(dumpTime); dumpPort.setEnvelope(dumpStamp); dumpPort.writeStrict(); yInfo()<<bDump.toString(); return true; }
void SamIter(void) { Bottle *b = bRead.read(false); //while (bRead.getPendingReads() > 0) // b = bRead.read(false); // get in the input from the port, if you want it to wait use true, else use false if(b!=NULL) // check theres data { if(b->size()>0) { task = b->get(0).asString(); int iStatus=atoi(b->get(1).asString().c_str()); //check if message is related to charging if(task.compare("ChargingStatus")==0) { //relay switch: off mobile base and close modules if(iStatus==0) { yarp::os::Time::delay(10); CPhidgetInterfaceKit_setOutputState (ifKit, 6, 0); system("stop.bat"); bCharging=true; } else if(iStatus==1 && bCharging==true)//relay switch: on mobile base and start modules { CPhidgetInterfaceKit_setOutputState (ifKit, 6, 1); yarp::os::Time::delay(2); system("start.bat"); bCharging=false; } } else { iflag = iStatus; //start timer if(iflag==1) time (&start); } } std::cout << "got a task " << b->toString().c_str() << std::endl; } //send phone ring sensor data only if sensor value is changed if(bPhoneRingChanged) { // send back a bottle with current voltage value Bottle& b3 = bPhone.prepare(); // prepare the bottle/port b3.clear(); b3.addInt( iPhoneRing ); // indicates robot voltage bPhone.writeStrict(); bPhoneRingChanged=false; } }
void saythis(std::string text) { Bottle& b=speechOut.prepare(); b.clear(); b.add(text.c_str()); speechOut.writeStrict(); log(now, text.c_str()); return; }
void log(double time, string strng) { stringstream logstream; logstream << setprecision(15) << time << " " << strng.c_str(); cout << logstream.str() << endl; Bottle& b=logOut.prepare(); b.clear(); b.add(logstream.str().c_str()); logOut.writeStrict(); }
void SamIter(void) { Bottle& B = bStarSend.prepare(); // prepare the bottle/port B.clear(); B.addInt(i_ID); B.addDouble(i_X); B.addDouble(i_Y); B.addDouble(i_Angle); bStarSend.writeStrict(); // add stuff then send }
int main() { // Initialize YARP - some OSes need network and time service initialization Network yarp; // Work locally - don't rely on name server (just for this example). // If you have a YARP name server running, you can remove this line. Network::setLocalMode(true); // Create two ports that we'll be using to transmit "Bottle" objects. // The ports are buffered, so that sending and receiving can happen // in the background. BufferedPort<Bottle> in; BufferedPort<Bottle> out; // we will want to read every message, with no skipping of "old" messages // when new ones come in in.setStrict(); // Name the ports in.open("/in"); out.open("/out"); // Connect the ports so that anything written from /out arrives to /in Network::connect("/out","/in"); // Send one "Bottle" object. The port is responsible for creating // and reusing/destroying that object, since it needs to be sure // it exists until communication to all recipients (just one in // this case) is complete. Bottle& outBot1 = out.prepare(); // Get the object outBot1.fromString("hello world"); // Set it up the way we want printf("Writing bottle 1 (%s)\n", outBot1.toString().c_str()); out.write(); // Now send it on its way // Send another "Bottle" object Bottle& outBot2 = out.prepare(); outBot2.fromString("2 3 5 7 11"); printf("Writing bottle 2 (%s)\n", outBot2.toString().c_str()); out.writeStrict(); // writeStrict() will wait for any // previous communication to finish; // write() would skip sending if // there was something being sent // Read the first object Bottle *inBot1 = in.read(); printf("Bottle 1 is: %s\n", inBot1->toString().c_str()); // Read the second object Bottle *inBot2 = in.read(); printf("Bottle 2 is: %s\n", inBot2->toString().c_str()); return 0; }
int LookAt(Point& center, Mat& CircleMat) // returning a number because we want the return code to be nice { Bottle *RiKinIn = iKinInPort.read(); // At first we read the Input Bottle (it will be a vector of doubles) if (RiKinIn != NULL) // Check if we got something { // ########################## // Current Focus of Attention // ########################## double z = RiKinIn->get(0).asDouble(); // read the coordinates where the robot currently looks double x = RiKinIn->get(1).asDouble(); double y = RiKinIn->get(2).asDouble(); circle(CircleMat, Point((160.0 + 640.0 * x), (360.0 - 600.0 * y)), 3, Scalar(0, 0, 255), 3, 8, 0); // Painting a red dot where the robot looks (and converting to the move_head reference) printf("Looking at: %.2f - %.2f \n", x, y); // printing where the robot looks Bottle &RiKinOut = iKinOutPort.prepare(); // prepare sending to the robot (and get a reference to the bottle) RiKinOut.clear(); // clear the cache // ###################### // New Focus of Attention // ###################### double NewX = (static_cast<double>(center.x) - 160.0) / 640.0; // convert where the Robot should look into the move_head reference double NewY = (static_cast<double>(center.y) - 360.0) / -600.0; double NewZ = -3.0; if ((abs(NewX - x) > 0.02) || (abs(NewY - y) > 0.02)) // We move the head only if the difference between the point where the robot currently looks { // and where we want the robot to look is big enough (this ensures not flooding the controller) RiKinOut.addDouble(NewZ); // Add our coordinates to the bottle RiKinOut.addDouble(NewX); RiKinOut.addDouble(NewY); iKinOutPort.writeStrict(); // writing to the output port and wait until writing is done printf("Look at: %.2f(%i) - %.2f(%i)\n", NewX, center.x, NewY, center.y); // inform the user were we want the robot to look at } } else { // if our bottle is empty :( then we should deal with it printf("Error in the iKinGazeCtrl Interface! Please restart the program and the iKinGazeCtrl Interface.\n"); printf("Press Return to Exit\n"); std::cin.get(); return ERROR_DEV_NOT_EXIST; } return ERROR_SUCCESS; }
void SamSendVoltage(void) { /* int iVolt=0; if(dVoltageNow<=12.4999)//require recharge iVolt=0; else if(dVoltageNow>=12.5000 && dVoltageNow<14.0000)//fine iVolt=1; else if (dVoltageNow>=14.0000)//fully charged iVolt=2; */ std::string s; // convert double b to string s std::ostringstream ss; ss << dVoltageNow; s = ss.str(); //if(iVoltage!=iVolt) //{ // send back a bottle with current voltage value Bottle& b2 = bRead.prepare();// prepare the bottle/port b2.clear(); //b2.addInt( iVolt );// indicates robot voltage b2.addString(s.c_str()); bRead.writeStrict(); //iVoltage=iVolt; //} std::cout << " voltage " << dVoltageNow << std::endl; }
bool updateModule() { ImageOf<PixelRgb> *imgIn=imgInPort.read(true); if (imgIn==NULL) return false; ImageOf<PixelRgb> &imgOut=imgOutPort.prepare(); imgOut=*imgIn; cv::Mat img=cv::cvarrToMat(imgOut.getIplImage()); Vector xa,oa; iarm->getPose(xa,oa); Matrix Ha=axis2dcm(oa); xa.push_back(1.0); Ha.setCol(3,xa); Vector pc; igaze->get2DPixel(camSel,xa,pc); cv::Point point_c((int)pc[0],(int)pc[1]); cv::circle(img,point_c,4,cv::Scalar(0,255,0),4); Vector analogs,encs(nEncs),joints; if (ianalog!=NULL) ianalog->read(analogs); iencs->getEncoders(encs.data()); for (int i=0; i<3; i++) { if (ianalog!=NULL) finger[i].getChainJoints(encs,analogs,joints); else finger[i].getChainJoints(encs,joints); finger[i].setAng(CTRL_DEG2RAD*joints); } for (int fng=0; fng<3; fng++) { deque<cv::Point> point_f; for (int i=-1; i<(int)finger[fng].getN(); i++) { Vector fc; igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3): finger[fng].getH(i,true).getCol(3)),fc); point_f.push_front(cv::Point((int)fc[0],(int)fc[1])); cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4); if (i>=0) { cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2); point_f.pop_back(); } else cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2); } } imgOutPort.writeStrict(); return true; }
/* SendImage(address to input image) This send a Mat to the Yarpview - waits until sending is finished */ void SendImage(Mat& input) { ImageOf<PixelBgr>& outp = imageOutPort.prepare(); // we prepare our output image and get a pointer to an image object outp.wrapIplImage(ConvertImage(input).getIplImage()); // At first we convert the image, the we getting an ipl image out of this and wrapping this into our output image imageOutPort.writeStrict(); // now we send the image, AND wait until its sent }
void SamIter(void) { Bottle *goal = GoalIn.read(false); //puts("running reader"); //while (GoalIn.getPendingReads() > 0) // goal = GoalIn.read(false); if(goal!=NULL) // check theres data { std::cout << "size of bottle is " << goal->size() << " data " << goal->toString().c_str() << std::endl; if(goal->size()>0) { //iGoalPostion =GetGoalFromString(goal->get(1).asString().c_str()); iGoalPostion=atoi(goal->get(1).asString().c_str()); SetGoalPos(iGoalPostion); bReached=false; std::cout<< "got a goal, location " << iGoalPostion <<std::endl; iStuckCount=0; loopCnt=0; robot->enableMotors(); robot->enableSonar(); } } //// send back a bottle with current voltage value //Bottle& b2 = PwrOut.prepare(); // prepare the bottle/port //b2.clear(); //b2.addDouble(robot->getRealBatteryVoltage()); // indicates robot voltage avg //PwrOut.writeStrict(); Bottle *input = NULL; //puts("running reader"); while (StarIn.getPendingReads() > 0) input = StarIn.read(false); if(input!=NULL) // check theres data { dStarId = input->get(0).asDouble(); //std::cout << "star id " << starID << std::endl; dX = input->get(1).asDouble(); dY = input->get(2).asDouble(); dTh = input->get(3).asDouble(); if(firstStarReading==false) { CurrentPose.setX(dX); CurrentPose.setY(dY); CurrentPose.setTh(dTh); OldPose = CurrentPose; firstStarReading=true; } iStarCntr++; if((OldPose.findDistanceTo(CurrentPose)<1.5 || iStarCntr>3) && dStarId!=0) { CurrentPose.setX(dX); CurrentPose.setY(dY); CurrentPose.setTh(dTh); OldPose = CurrentPose; iStarCntr=0; } //puts("got a msg"); //puts(input->toString()); //std::cout << " robot->getRealBatteryVoltageNow() " << " " << robot->getRealBatteryVoltageNow()<<std::endl; } if(bReached==false) { loopCnt++; double ang = atan2(GoalPose.getY() - CurrentPose.getY(), GoalPose.getX() - CurrentPose.getX()); double dist = GoalPose.findDistanceTo(CurrentPose); //check if the robot is stuck while navigating if(dist>0) { //first distance reading if(iStuckCount==0) OldDist=dist; DiffDist=OldDist-dist; if(loopCnt%5==0 && abs(DiffDist)<0.1) { iStuckCount++; std::cout << "diff dist, dist, stuck count " << DiffDist << " " << dist << " " << iStuckCount <<std::endl; } else OldDist=dist; //recover motion if(iStuckCount>6) { robot->lock(); robot->setVel(-100);//move back yarp::os::Time::delay(5); robot->unlock(); //robot->setVel(0);//move back iStuckCount=0; } } //calculate_distance(dX, dY, GoalPose.getX(), GoalPose.getY());//ar.findDistanceTo(ArPose(-0.75,3.0,4.5)); ang-=dTh; while(ang>3.14159265){ang-=6.28318531;} // normalise PI ie if over 180, then minus 360 while(ang<-3.14159265){ang+=6.28318531;} ang = Rad2Deg(ang); //std::cout << "required angle, dist " << ang << " " << dist <<std::endl; //if the robot has to turn more then turn threshold for obst avoidance is less and speed is 0 if(abs(ang)>55) { myTurnThreshold = 250; myStopDistance = 200; speed=0; } else { speed=220; myTurnThreshold = 550; myStopDistance = 450; } bool obst= ObstacleAvoidance(); if(dist>=0.7 && obst==false)//&& abs(ang)>10 { robot->lock(); std::cout <<"navigating to goal " << iGoalPostion<<std::endl; /* if(abs(ang)<45) speed=200; else speed=0;*/ robot->setDeltaHeading(ang/2.0); robot->setVel(speed); robot->unlock(); } else if(dist<0.7) { robot->lock(); double rotFinal = GoalPose.getTh(); rotFinal-=dTh; std::cout <<"goal rot " << rotFinal <<std::endl; while(rotFinal>3.14159265){rotFinal-=6.28318531;} // normalise PI ie if over 180, then minus 360 while(rotFinal<-3.14159265){rotFinal+=6.28318531;} rotFinal = Rad2Deg(rotFinal); robot->setDeltaHeading(rotFinal); robot->setVel(0); std::cout <<"final rot " << rotFinal <<std::endl; //robot->setDeltaHeading(0); ang=0; speed=0; puts("reached "); bReached=true; robot->unlock(); // send back a bottle with goal position reached Bottle& b2 = GoalIn.prepare(); // prepare the bottle/port b2.clear(); b2.addInt(iGoalPostion); // indicates robot reached goal location GoalIn.writeStrict(); //wait for some time for rotation to complete yarp::os::Time::delay(5); robot->disableMotors(); robot->disableSonar(); } /*myfile.open ("motion.txt"); if (myfile.is_open()) { myfile << ang << "|" << speed << "\n"; myfile.close(); }*/ } //else{puts("didn't get a msg");} }