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); }
vector<Place> PlacesManager::findRandom(unsigned int limit) { vector<Place> places; ResultSet &rs = Database::getInstance().execute("SELECT id, name FROM place ORDER BY RANDOM() LIMIT %d", limit); while (rs.hasNext()) { Place place; place.setId(rs.getInt(0)); place.setName(rs.getString(1)); places.push_back(place); } return places; }
/**Operational method for the class; * Called to prompt the user to specificy the Place Object's attribute they wish to change, and * the value to set it to. * * @return Returns a pointer to itself (this) */ ModifyPlacesView* ModifyPlacesView::start() { Place* place = getPlace(rootPlace); string opts[3] = {"Name", "Longitude", "Latitude"}; OptionsView<string> view(opts, 3); int menuSelection = 0; do { cout << LF << "\tWhat would you like to modify?" << LF; switch (menuSelection = view.display()->getOption()) { case 0: place->setName(getAddress(1, "Please enter the name")); break; case 1: place->setLongitude(inputFloat("Please enter the longitude")); break; case 2: place->setLatitude(inputFloat("Please enter the latitude")); break; } } while (menuSelection != 3); return this; }
bool PNPGenerator::genFromPolicy(Policy &p) { bool _error = false; // check if errors occur during generation // Map of visited states (with associated places) std::map<string,Place*> visited; cout << "Init: " << p.initial_state << endl; string current_state = p.initial_state; // add a first fixed transition from the init place to the initial state pair<Transition*,Place*> pa = pnp.addCondition("[]",pnp.pinit); Place *p1 = pa.second; p1->setName(current_state); // put place for initial state visited[p.initial_state]=p1; cout << "Final: " << p.final_state << endl; //string final_state_str = transformState(final_state); // Initialization of the stack stack< pair<string, Place*> > SK; SK.push(make_pair(current_state,p1)); while (!SK.empty()) { // get top element from stack current_state=SK.top().first; Place* current_place = SK.top().second; SK.pop(); std::cout << "PNPgen:: " << current_state << " : "; string action = p.getActionForState(current_state); if (action=="") { if (current_state!=p.final_state) std::cerr << "PNPgen Warning: No action found for state " << current_state << std::endl; continue; } std::cout << action << " -> "; vector<StateOutcome> vo = p.getOutcomes(current_state,action); if (vo.size()==0) { std::cerr << "PNPgen ERROR: No successor state found for state " << current_state << " and action " << action << std::endl; _error = true; break; } Place *pe = addAction(action,current_place); // pe: end place of action // y coordinate of Petri Net layout int dy=0; vector<StateOutcome>::iterator io; for (io = vo.begin(); io!=vo.end(); io++) { string succ_state = io->successor; string cond = io->observation; if (cond[0]!='[') cond = "["+cond+"]"; cout << cond << " " << succ_state << " "; Place *ps = visited[succ_state]; // check if succ_state is already visited if (ps==NULL) { // if not visited pair<Transition*,Place*> pa = pnp.addCondition(cond,pe,dy); dy++; Place* pc = pa.second; SK.push(make_pair(succ_state,pc)); visited[succ_state]=pc; pc->setName(succ_state); if (succ_state==p.final_state) pc->setName("goal"); } else { // if already visited pnp.addConditionBack(cond,pe, ps, dy); dy++; } } // for io std::cout << std::endl; } return !_error; }
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::setMainLinearPlan(string plan) { Place *p = genLinearPlan(pnp.pinit,plan); p->setName("goal"); }
void T41::policyCallback(const t41_robust_navigation::Policy::ConstPtr& msg) { std::map<string,string> policy; std::map<string,Place*> visited; std::map<std::pair<string,string>,vector<string> > transition_fn; std::map<string,string> transformedconditions; string initial_state = msg->initial_state; final_state = msg->final_state; goalname = msg->goal_name; ROS_INFO("Received policy for goal %s. Initial state: %s Final state: %s", goalname.c_str(), initial_state.c_str(), final_state.c_str()); std::vector<t41_robust_navigation::StatePolicy> p = msg->policy; t41_robust_navigation::StatePolicy sa; std::vector<t41_robust_navigation::StatePolicy>::iterator it = p.begin(); while (it!=p.end()) { sa = *it++; // printf(" %s : %s -> ", sa.state.c_str(), sa.action.c_str()); string tstate = transformState(sa.state); //transformedconditions[tstate] = extractCondition(sa.state); string aname = extractName(sa.action); string aparam = transformParamsWith_(extractParams(sa.action)); string taction = aname; if (aparam!="") taction = aname+"_"+aparam; policy[tstate] = taction; //cout << "### Added policy " << tstate << " -> " << taction << endl; // vector<string> ss = sa.successors; vector<t41_robust_navigation::StateOutcome> &so = sa.outcomes; // vector<string>::iterator is; // for (is=ss.end(); is!=ss.begin(); ) { for (vector<t41_robust_navigation::StateOutcome>::const_iterator is=so.end(); is!=so.begin(); ) { is--; // string succ = *is; string succ = is->successor; string tsucc = transformState(succ); transformedconditions[tsucc] = extractCondition(is->observation); // Not really sure of this, condition should be attached to source state instead of destination state I think //printf(" %s [%s] ",succ.c_str(),tsucc.c_str()); transition_fn[make_pair(tstate,taction)].push_back(tsucc); //cout << "### Added transition " << tstate << "," << taction << " -> " << tsucc << endl; } //printf("\n"); } // Generates the PNP PNP pnp("policy"); Place *p0 = pnp.addPlace("init"); p0->setInitialMarking(); string current_state = transformState(initial_state); bool PNPgen_error = false; transformedconditions[current_state] = "[true]"; pair<Transition*,Place*> pa = pnp.addCondition(transformedconditions[current_state],p0); Place *p1 = pa.second; p1->setName(current_state); stack< pair<string, Place*> > SK; SK.push(make_pair(current_state,p1)); while (!SK.empty()) { current_state=SK.top().first; Place* current_place = SK.top().second; SK.pop(); std::cout << "PNPgen:: " << current_state << ": "; string action = policy[current_state]; if (action=="") { std::cerr << "ERROR. No action found at this point!!!" << std::endl; PNPgen_error = true; break; } std::cout << action << " -> "; vector<string> vs = transition_fn[std::make_pair(current_state,action)]; if (vs.size()==0) { std::cerr << "ERROR. No successor state found at this point!!!" << std::endl; PNPgen_error = true; break; } Place *pe = pnp.addAction(action,current_place); vector<string>::iterator iv; int dy=0; // Ordering successor states (transitions) wrt number of conditions int maxn=0; // vector<string> conditions[32]; // max number of atomic conditions vector<string> succstates[32]; for (iv = vs.begin(); iv!=vs.end(); iv++) { string succ_state = *iv; string cond = transformedconditions[succ_state]; cout << "Ordering conditions: " << cond << " "; int n = count_conditions(cond); if (n>maxn) maxn=n; cout << n << endl; // conditions[n].push_back(cond); succstates[n].push_back(succ_state); } for (int k=maxn; k>0; k--) { // vector<string> vc = conditions[k]; vector<string> ss = succstates[k]; for (int i = 0; i<ss.size(); i++) { string succ_state = ss[i]; std::cout << succ_state << " "; Place *ps = visited[succ_state]; int x = pe->getX(); // x position for all the conditions if (ps==NULL) { pair<Transition*,Place*> pa = pnp.addCondition(transformedconditions[succ_state],pe,dy); dy++; Place* pc = pa.second; SK.push(make_pair(succ_state,pc)); visited[succ_state]=pc; pc->setName(succ_state); if (succ_state==final_state) pc->setName("goal"); } else { pnp.addConditionBack(transformedconditions[succ_state],pe, ps, dy); dy++; } } // for i } // for k std::cout << std::endl; } if (PNPgen_error) { t41_robust_navigation::PolicyResult res; res.goal_name = goalname; res.feedback = "FAILURE: cannot generate a PNP"; res.state = initial_state; policyresult_pub.publish(res); return; } string planname = "AUTOGENpolicy"; string pnpfilename = planfolder+"/"+planname+".pnml"; std::ofstream of(pnpfilename.c_str()); of << pnp; of.close(); ROS_INFO_STREAM("Saved PNP file " << pnpfilename); // publish planToExec to start the plan std_msgs::String s; //s.data = "stop"; //plantoexec_pub.publish(s); // stop the current plan //boost::this_thread::sleep(boost::posix_time::milliseconds(500)); s.data = planname; plantoexec_pub.publish(s); // start the new one }