C++ Place::getX方法代码示例

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

}

//.........这里部分代码省略.........

    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


}