void ompl::geometric::BasicPRMmodif::reconstructLastSolution(void)
{
if (lastStart_ && lastGoal_)
constructSolution(lastStart_, lastGoal_);
else
msg_.warn("There is no solution to reconstruct");
}
ompl::base::PlannerStatus ompl::geometric::LazyPRM::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add the valid start states as milestones
while (const base::State *st = pis_.nextStart())
startM_.push_back(addMilestone(si_->cloneState(st)));
if (startM_.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
// Ensure there is at least one valid goal state
if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
{
const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
goalM_.push_back(addMilestone(si_->cloneState(st)));
if (goalM_.empty())
{
OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
}
unsigned long int nrStartStates = boost::num_vertices(g_);
OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
bestCost_ = opt_->infiniteCost();
base::State *workState = si_->allocState();
std::pair<std::size_t, std::size_t> startGoalPair;
base::PathPtr bestSolution;
bool fullyOptimized = false;
bool someSolutionFound = false;
unsigned int optimizingComponentSegments = 0;
// Grow roadmap in lazy fashion -- add vertices and edges without checking validity
while (ptc == false)
{
++iterations_;
sampler_->sampleUniform(workState);
Vertex addedVertex = addMilestone(si_->cloneState(workState));
const long int solComponent = solutionComponent(&startGoalPair);
// If the start & goal are connected and we either did not find any solution
// so far or the one we found still needs optimizing and we just added an edge
// to the connected component that is used for the solution, we attempt to
// construct a new solution.
if (solComponent != -1 && (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
{
// If we already have a solution, we are optimizing. We check that we added at least
// a few segments to the connected component that includes the previously found
// solution before attempting to construct a new solution.
if (someSolutionFound)
{
if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
continue;
optimizingComponentSegments = 0;
}
Vertex startV = startM_[startGoalPair.first];
Vertex goalV = goalM_[startGoalPair.second];
base::PathPtr solution;
do
{
solution = constructSolution(startV, goalV);
} while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
if (solution)
{
someSolutionFound = true;
base::Cost c = solution->cost(opt_);
if (opt_->isSatisfied(c))
{
fullyOptimized = true;
bestSolution = solution;
bestCost_ = c;
break;
}
else
{
if (opt_->isCostBetterThan(c, bestCost_))
{
bestSolution = solution;
bestCost_ = c;
}
}
}
}
}
si_->freeState(workState);
if (bestSolution)
{
base::PlannerSolution psol(bestSolution);
psol.setPlannerName(getName());
// if the solution was optimized, we mark it as such
psol.setOptimized(opt_, bestCost_, fullyOptimized);
pdef_->addSolutionPath(psol);
}
OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
return bestSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
bool ompl::geometric::BasicPRMmodif::solve(const base::PlannerTerminationCondition &ptc)
{
pis_.checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
msg_.error("Goal undefined or unknown type of goal");
return false;
}
std::vector<Milestone*> startM;
std::vector<Milestone*> goalM;
// add the valid start states as milestones
while (const base::State *st = pis_.nextStart())
startM.push_back(addMilestone(si_->cloneState(st)));
if (startM.size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!goal->canSample())
{
msg_.error("Insufficient states in sampleable goal region");
return false;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
unsigned int nrStartStates = milestones_.size();
msg_.inform("Starting with %u states", nrStartStates);
base::State *xstate = si_->allocState();
std::pair<Milestone*, Milestone*> solEndpoints;
while (ptc() == false)
{
// find at least one valid goal state
if (goal->maxSampleCount() > goalM.size())
{
const base::State *st = goalM.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
goalM.push_back(addMilestone(si_->cloneState(st)));
if (goalM.empty())
{
msg_.error("Unable to find any valid goal states");
break;
}
}
// if there already is a solution, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
// othewise, spend some time building a roadmap
else
{
// if it is worth looking at other goal regions, plan for part of the time
if (goal->maxSampleCount() > goalM.size())
growRoadmap(startM, goalM, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate);
// otherwise, just go ahead and build the roadmap
else
growRoadmap(startM, goalM, ptc, xstate);
// if a solution has been found, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
}
}
if (haveSolution(startM, goalM, &solEndpoints))
{
constructSolution(solEndpoints.first, solEndpoints.second);
}
si_->freeState(xstate);
msg_.inform("Created %u states", milestones_.size() - nrStartStates);
return goal->isAchieved();
}
int main(int argc, char **argv)
{
//*****************************SET PARAMETERS*************************************************
Parameters params(argc, argv, 0);
Parameters params2(argc, argv, 1);
//********************************************************************************************
//**********************READ DATA AND MAKE SOLUTION*******************************************
Solution* sol = constructSolution(params);
preprocesData(sol);
Solution* sol2 = constructSolution(params2);
preprocesData(sol2);
//*********************************************************************************************
//*****************************SET PARAMETERS*************************************************
if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() <= 100 * 1000) {
params2.delta = params.delta = 40;
params2.nmb_iters_bpr = params.nmb_iters_bpr = 300;
params2.nmbRanges = params.nmbRanges = 7;
params2.rangeLength = params.rangeLength = sol->getNumberOfProcesses() / params.nmbRanges;
}
else {
params2.nmb_iters_bpr = params.nmb_iters_bpr = 100;
params2.nmbRanges = params.nmbRanges = 5;
params2.rangeLength = params.rangeLength = sol->getNumberOfProcesses() / params.nmbRanges;
params2.numberOfBestProcessesToConsiderShiftSwap =
params.numberOfBestProcessesToConsiderShiftSwap = 3;
params2.nmbLoops = params.nmbLoops = 2;
if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() >= 20000 * 2000)
params2.nmbLoops = params.nmbLoops = 1;
}
params2.seedValue = params.seedValue + 1000;
//********************************************************************************************
if(sol->getNumberOfProcesses() * sol->getNumberOfMachines() <= 100 * 1000) // A instances
solveA(params, sol, params2, sol2);
else // B instances
solveB(params, sol, params2, sol2);
long timeToSleep = params.time_limit - (time(0) - params.programStartTime) - 3;
if (timeToSleep < 0)
timeToSleep = 0;
sleep(timeToSleep);
// Read Solutions From Files And Write a better one
sol->setOriginalLoadCostWeights();
sol2->setOriginalLoadCostWeights();
//cout << " sol->solutionFilename " << sol->solutionFilename << endl;
//cout << " sol2->solutionFilename " << sol2->solutionFilename << endl;
//cout << " sol->getCost() " << sol->getCost() << endl;
//cout << " sol2->getCost() " << sol2->getCost() << endl;
readSolutionFromFile( sol, sol->solutionFilename);
readSolutionFromFile(sol2, sol2->solutionFilename);
//cout << " after sol->getCost() " << sol->getCost() << endl;
//cout << " after sol2->getCost() " << sol2->getCost() << endl;
if ((sol->getCost() < sol2->getCost()) && (sol->checkConflict(false)))
sol->writeToFile(params.solution_filename);
else
sol2->writeToFile(params.solution_filename);
//remove solution files
ostringstream oss1(""); oss1 << "rm " << sol->solutionFilename;
system(oss1.str().c_str());
ostringstream oss2(""); oss2 << "rm " << sol2->solutionFilename;
system(oss2.str().c_str());
//*****************************************FILES (REMOVE THIS PART)********************************************************
/*fstream fileResults("results", ios::out | ios::app);
fileResults << setw(5) << params.seedValue << setw(30) << params.data_filename
<< setw(20) << sol->getCost()
<< setw(20) << sol2->getCost()
<< setw(20) << *(params.best_objective)
<< setw(15) << time(0) - params.programStartTime << endl;
// running times
fstream fileRunnungTimes("runnung_times", ios::out | ios::app);
fileRunnungTimes << setw(5) << setw(30) << params.data_filename << setw(20) << *(params.best_objective)
<< setw(20) << time(0) - params.programStartTime << endl;
// check solution with official checker
ostringstream oss("");
oss << "./checker " << params.data_filename << " " << params.initial_assignment_filename
<< " " << params.solution_filename;
oss << " >> checkerFile ";
system(oss.str().c_str());
*/
//*************************************************************************************************************************
return 0;
}
