void PedestrianDirectionsEngine::Generate(IRoadGraph const & graph, vector<Junction> const & path, Route::TTimes & times, Route::TTurns & turns, vector<Junction> & routeGeometry, my::Cancellable const & cancellable) { times.clear(); turns.clear(); routeGeometry.clear(); if (path.size() <= 1) return; CalculateTimes(graph, path, times); vector<Edge> routeEdges; if (!ReconstructPath(graph, path, routeEdges, cancellable)) { LOG(LDEBUG, ("Couldn't reconstruct path.")); // use only "arrival" direction turns.emplace_back(path.size() - 1, turns::PedestrianDirection::ReachedYourDestination); return; } CalculateTurns(graph, routeEdges, turns, cancellable); routeGeometry = path; }
vector<int> ADPlanner::GetSearchPath(ADSearchStateSpace_t* pSearchStateSpace, int& solcost) { vector<int> SuccIDV; vector<int> CostV; vector<int> wholePathIds; ADState* searchstateinfo; CMDPSTATE* state = NULL; CMDPSTATE* goalstate = NULL; CMDPSTATE* startstate=NULL; #if AD_SEARCH_FORWARD == 1 startstate = pSearchStateSpace->searchstartstate; goalstate = pSearchStateSpace->searchgoalstate; //reconstruct the path by setting bestnextstate pointers appropriately if(ReconstructPath(pSearchStateSpace) != 1){ solcost = INFINITECOST; return wholePathIds; } #else startstate = pSearchStateSpace->searchgoalstate; goalstate = pSearchStateSpace->searchstartstate; #endif #if DEBUG PrintSearchPath(pSearchStateSpace, fDeb); #endif state = startstate; wholePathIds.push_back(state->StateID); solcost = 0; FILE* fOut = stdout; int steps = 0; const int max_steps = 10000; while(state->StateID != goalstate->StateID && steps < max_steps) { steps++; if(state->PlannerSpecificData == NULL) { fprintf(fOut, "path does not exist since search data does not exist\n"); break; } searchstateinfo = (ADState*)state->PlannerSpecificData; if(searchstateinfo->bestnextstate == NULL) { fprintf(fOut, "path does not exist since bestnextstate == NULL\n"); break; } if(searchstateinfo->g == INFINITECOST) { fprintf(fOut, "path does not exist since bestnextstate == NULL\n"); break; } environment_->GetSuccs(state->StateID, &SuccIDV, &CostV); int actioncost = INFINITECOST; for(int i = 0; i < (int)SuccIDV.size(); i++) { if(SuccIDV.at(i) == searchstateinfo->bestnextstate->StateID) actioncost = CostV.at(i); } solcost += actioncost; //fprintf(fDeb, "actioncost=%d between states %d and %d\n", // actioncost, state->StateID, searchstateinfo->bestnextstate->StateID); //environment_->PrintState(state->StateID, false, fDeb); //environment_->PrintState(searchstateinfo->bestnextstate->StateID, false, fDeb); state = searchstateinfo->bestnextstate; wholePathIds.push_back(state->StateID); } if(state->StateID != goalstate->StateID){ printf("ERROR: Failed to getsearchpath, steps processed=%d\n", steps); wholePathIds.clear(); solcost = INFINITECOST; return wholePathIds; } return wholePathIds; }