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);
}
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
}
