void PNPGenerator::genHumanAction(string say_ask, string say_do, string action_do, string say_dont, string condition) { Place *p1 = pnp.addAction("findHuman",pnp.pinit); addActionToStacks("findHuman",pnp.pinit); Place *p2 = pnp.addAction(say_ask,p1); addActionToStacks(say_ask,p1); pair<Transition*,Place*> ptg = pnp.addCondition(" ",p2); Place *p = ptg.second; Transition* t1 = pnp.addTransition("["+condition+"]"); t1->setY(p->getY()-1); t1->setX(p->getX()+1); Transition* t2 = pnp.addTransition("[saidYes]"); t2->setY(p->getY()+0); t2->setX(p->getX()+1); Transition* t3 = pnp.addTransition("[saidNo]"); t3->setY(p->getY()+1); t3->setX(p->getX()+1); Transition* t4 = pnp.addTransition("[timeout]"); t4->setY(p->getY()+2); t4->setX(p->getX()+1); pnp.connect(p,t1); pnp.connect(p,t2); pnp.connect(p,t3); pnp.connect(p,t4); Place *py = pnp.addPlace("Y",-1); py->setY(t2->getY()); py->setX(t2->getX()+1); pnp.connect(t2,py); Place *pn = pnp.addPlace("N",-1); pn->setY(t3->getY()); pn->setX(t3->getX()+1); pnp.connect(t3,pn); // say_do, action_do Place *pd1 = pnp.addAction(say_do,py); addActionToStacks(say_do,py); Place *pd2 = pnp.addAction(action_do,pd1); addActionToStacks(action_do,pd1); ptg = pnp.addCondition("["+condition+"]",pd2); Place *pg = ptg.second; pg->setName("goal"); // say_dont Place *pf1 = pnp.addAction(say_dont,pn); addActionToStacks(say_dont,pn); Transition* tf1 = pnp.addTransition("[not humanDetected]"); tf1->setY(pf1->getY()); tf1->setX(pf1->getX()+1); pnp.connect(pf1,tf1); pnp.connect(tf1,pnp.pinit); pnp.connect(t1,pg); pnp.connect(t4,pnp.pinit); pnp.addInterrupt(pnp.pinit, condition, pg); }
Place* PNP::addAction(string name, Node *p0) { Transition *ts = addTransition(name+".start"); Place *pe = addPlace(name+".exec"); Transition *te = addTransition(name+".end"); Place *pf = addPlace("X",-1); ts->setY(p0->getY()); pe->setY(p0->getY()); // same line as p0 te->setY(p0->getY()); pf->setY(p0->getY()); ts->setX(p0->getX()+1); pe->setX(p0->getX()+2); // X pos after p0 te->setX(p0->getX()+3); pf->setX(p0->getX()+4); connect(p0,ts); connect(ts,pe); connect(pe,te); connect(te,pf); nactions++; return pf; }
Place* PNP::addTimedAction(string name, Place *p0, int timevalue, Place **p0action) { // fork transition Transition *tf = addTransition("[]"); tf->setX(p0->getX()+1); tf->setY(p0->getY()); connect(p0,tf); // initial places of actions Place *pi1 = addPlace("X",-1); pi1->setX(tf->getX()+1); pi1->setY(tf->getY()-1); connect(tf,pi1); Place *pf1 = addAction(name,pi1); // actions Place *pi2 = addPlace("X",-1); pi2->setX(tf->getX()+1); pi2->setY(tf->getY()+1); connect(tf,pi2); stringstream ss; ss << "wait_" << timevalue; Place *pf2 = addAction(ss.str(),pi2); // join transition Transition *tj = addTransition("[]"); tj->setX(pf1->getX()+1); tj->setY(pf1->getY()+1); connect(pf1,tj); connect(pf2,tj); // interrupt Transition *ti = addTransition(name+".interrupt []"); ti->setX(tj->getX()); ti->setY(tj->getY()+1); connect(next(next(pi1)),ti); connect(pf2,ti); // final place Place *po = addPlace("X",-1); po->setX(tj->getX()+1); po->setY(tj->getY()); connect(tj,po); connect(ti,po); nactions+=2; *p0action = pi1; // initial place of action return po; }
vector<Place*> PNP::addSensingAction(string name, Place* p0, vector<string> outcomes) { vector<Place *> v; Transition *ts = addTransition(name+".start"); Place *pe = addPlace(name+".exec"); ts->setY(p0->getY()); pe->setY(p0->getY()); // same line as p0 ts->setX(p0->getX()+1); pe->setX(p0->getX()+2); // X pos after p0 connect(p0,ts); connect(ts,pe); int k=0; for (vector<string>::iterator it = outcomes.begin(); it!=outcomes.end(); it++, k++) { Transition *te = addTransition("["+(*it)+"] "+name+".end"); Place *pf = addPlace("X",-1); te->setY(p0->getY()+k); pf->setY(p0->getY()+k); te->setX(p0->getX()+3); pf->setX(p0->getX()+4); connect(pe,te); connect(te,pf); v.push_back(pf); } nactions++; return v; }
void PNPGenerator::applyExecutionRules() { pair<string, Place*> current; Place* noplace=NULL; while (!ASE.empty()) { current=ASE.top(); ASE.pop(); string current_action_param = current.first; vector<string> tk; boost::split(tk,current_action_param,boost::is_any_of("_")); string current_action = tk[0]; // current action (without parameters) Place* current_place = current.second; // init place of this action cout << "Applying execution rules for action " << current_action << endl; vector<TExecutionRule>::iterator eit; eit = executionrules.v.begin(); while (eit!=executionrules.v.end()) { if (eit->action==current_action) { cout << " " << eit->condition << " -> " << eit->recoveryplan << endl; boost::trim(eit->recoveryplan); vector<string> v; boost::split(v,eit->recoveryplan,boost::is_any_of("; "),boost::token_compress_on); Place *po = NULL; string R=""; Place *pi = pnp.addPlace("I",-1); pi->setY(current_place->getY()-1); pi->setX(current_place->getX()+3); if (v.size()>1) { // build actual plan without recovery specification string plan = ""; int i=0; for (i=0; i<v.size()-2; i++) plan = plan + v[i] + ";"; plan = plan + v[i]; //cout << "-- recovery plan " << plan << endl; po = genLinearPlan(pi,plan,false); // output place of linear plan R = v[v.size()-1]; } else { R = eit->recoveryplan; po = pi; } pnp.addInterrupt(current_place,eit->condition,pi); if (R=="fail_plan") { po->setName("fail"); } else if (R=="restart_plan") { pnp.connectPlaces(po,pnp.pinit); } else if (R=="restart_action") { pnp.connectPlaces(po,current_place); } else if (R=="skip_action") { pnp.connectPlaces(po,pnp.endPlaceOf(current_place)); } } eit++; } } }
void PNPGenerator::applySocialRules() { pair<string, Place*> current; Place* noplace=NULL; while (!ASS.empty()) { current=ASS.top(); ASS.pop(); string current_action_param = current.first; vector<string> tk; boost::split(tk,current_action_param,boost::is_any_of("_")); string current_action = tk[0]; // current action (without parameters) Place* current_place = current.second; // init place of this action // before vector<pair<string,string> >::iterator sit = socialrules.begin(); while (sit!=socialrules.end()) { string a = sit->first, b = sit->second; sit++; vector<string> tk; boost::split(tk,a,boost::is_any_of(" ")); if (tk[0]=="before" && tk[1]==current_action) { current_place=add_before(pnp,b,current_action,current_place); /* - Execution rules if (b=="approach") pnp.addInterrupt(current_place,"not humanDetected",pnp.pinit); if (b=="explain_approach") pnp.addInterrupt(current_place,"not humanDetected",pnp.pinit); */ } } // after sit = socialrules.end(); while (sit!=socialrules.begin()) { sit--; string a = sit->first, b = sit->second; vector<string> tk; boost::split(tk,a,boost::is_any_of(" ")); if (tk[0]=="after" && tk[1]==current_action) { Place *pp = add_after(pnp,b,current_action,current_place); /* - Execution rules if (b=="approach") pnp.addInterrupt(pp,"not humanDetected",pnp.pinit); // not humanDetected if (b=="explain_approach") pnp.addInterrupt(current_place,"not humanDetected",pnp.pinit); */ } } // during sit = socialrules.begin(); while (sit!=socialrules.end()) { string a = sit->first, b = sit->second; sit++; vector<string> tk; boost::split(tk,a,boost::is_any_of(" ")); if (tk[0]=="during" && tk[1]==current_action) { // add b during this action //cout << "-- add " << b << " in parallel with " << current_action << endl; // search for action after place current_place // it may be different when after and before add actions around... Node* nn = current_place; bool found=false; while (!found) { string naction = pnp.next(nn)->getName(); found = naction.substr(0,current_action.size())==current_action; // cout << naction << "=" << current_action << " " << found << endl; if (!found) nn = pnp.next(nn); } Node* t1 = pnp.disconnect(nn); Transition* tf = pnp.addTransition(" "); tf->setX(nn->getX()+1); tf->setY(nn->getY()); pnp.connect(nn,tf); Place* p1 = pnp.addPlace("YA",-1); p1->setX(tf->getX()+1); p1->setY(tf->getY()); pnp.connect(tf,p1); pnp.connect(p1,t1); current_place=p1; Place* p2 = pnp.addPlace("YB",-1); p2->setX(tf->getX()+1); p2->setY(tf->getY()+1); pnp.connect(tf,p2); Place* p2e = pnp.addAction(b,p2); addActionToStacks(b,p2); // new action (b, p2) to be analyzed Transition* tj = pnp.addTransition(" "); tj->setX(p2e->getX()+1); tj->setY(p2e->getY()); pnp.connect(p2e,tj); for (int k=0; k<3; k++) t1 = pnp.next(t1); Node* t3 = pnp.disconnect(t1); pnp.connect(t1,tj); Place *po = pnp.addPlace("Z",-1); po->setX(tj->getX()+2); po->setY(tj->getY()); pnp.connect(tj,po); pnp.connect(po,t3); } } } }
void startInteractiveTest() { float l, w, d, h; int mD; cout << "\tQuadtree testing - Interactive test" << endl << endl << "Enter parameters for new tree (left, width, down, height, max depth)" << endl; cin >> l >> w >> d >> h >> mD; { Quadtree testTree(l, w, d, h, mD); map<string, Place *> places; int testOpt; CLEAR_SCR(); try { do { cout << "\tQuadtree testing - Interactive test" << endl << endl << "1. Print testing tree" << endl << "2. Add new place" << endl << "3. Remove place" << endl << "4. Move place" << endl << "5. Print places" << endl << "6. Get place at coordinate" << endl << "7. Get places in area" << endl << endl << "0. Exit interactive test" << endl << endl << "Enter option: "; cin >> testOpt; CLEAR_SCR(); switch (testOpt) { case PRINT_TREE: { cout << testTree; PAUSE(); PAUSE(); //Needs two since "\n" is still in cin. break; } case PRINT_PLACES: { cout << "Allocated places:" << endl; for (map<string, Place *>::iterator it = places.begin(); it != places.end(); it++) { cout << "\t" << (*it).second->getName() << "\t(" << (*it).second->getX() << ", " << (*it).second->getY() << ")" << endl; } PAUSE(); PAUSE(); break; } case ADD_PLACE: { float x, y; string name; cout << "Enter name: "; cin >> name; cout << "Coordinate: "; cin >> x >> y; Place *newPlace = new Place(x, y, name); testTree.addPos(newPlace); places[name] = newPlace; PAUSE(); PAUSE(); break; } case REMOVE_PLACE: { string name; Place *place; cout << "Enter name: "; cin >> name; place = places[name]; testTree.removePos(place); places.erase(name); PAUSE(); PAUSE(); break; } case MOVE_PLACE: { string name; Place *place; float x, y; cout << "Enter name: "; cin >> name; cout << "Enter new coordinate: "; cin >> x >> y; place = places[name]; place->setX(x); place->setY(y); testTree.updatePos(place); PAUSE(); PAUSE(); break; } case GET_AT: { float x, y; cout << "Enter coordinate: "; cin >> x >> y; vector<IRO_Point2D *> points; points = testTree.getContentAt(x, y); cout << "Found: " << points.size() << " places." << endl; for (vector<IRO_Point2D *>::iterator it = points.begin(); it != points.end(); it++) { cout << static_cast<Place *>(*it)->getName() << " "; } PAUSE(); PAUSE(); break; } case GET_IN_RECT: { float x1, y1, x2, y2; vector<IRO_Point2D *> placesInRect; cout << "Enter rectangle (left, down, right, up): "; cin >> x1 >> y1 >> x2 >> y2; placesInRect = testTree.getContentInRect(x1, y1, x2, y2); cout << "Found: " << placesInRect.size() << " places." << endl; for (vector<IRO_Point2D *>::iterator it = placesInRect.begin(); it != placesInRect.end(); it++) { cout << static_cast<Place *>(*it)->getName() << " "; } PAUSE(); PAUSE(); break; } } CLEAR_SCR(); } while (testOpt != EXIT_TEST); } catch (exception &e) { cout << e.what() << endl; PAUSE(); PAUSE(); } for (map<string, Place *>::iterator it = places.begin(); it != places.end(); it++) { delete (*it).second; } places.clear(); } PAUSE(); }