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