void Strategy_Planner::plan(bool is_test_mode, int planner, int navigator) {
if (!is_test_mode) {
if (!is_emergency_) {
setPlanner(a_star_seed);
} else {
setPlanner(quick_reflex);
}
if (nml_flag) {
setNavigator(waypoint_navigator);
} else {
if (is_confident_) {
setNavigator(lane_navigator);
} else {
setNavigator(waypoint_navigator);
}
}
} else {
setPlanner(planner);
setNavigator(navigator);
}
}
void DensoSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<DensoStateSpace>()->
_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
if (_useRealScene)
{
startVec[0] = -1.576;
startVec[1] = 1.374;
startVec[2] = 0.0;
startVec[3] = 0.0;
startVec[4] = -1.36751533;
startVec[5] = 1.615;
goalVec[0] = 1.55;
goalVec[1] = 1.35;
goalVec[2] = 0.1;
goalVec[3] = -000057;
goalVec[4] = -1.40754;
goalVec[5] = 1.6;
}
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
/* set start, goal, and threshold*/
setStartAndGoalStates(start, goal, threshold);
/* set propagation step size and min-max propagation steps*/
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
/* initialize a new KPIECE planner */
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
/* set default parameters */
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
/* set a state validity checker */
setStateValidityChecker(ob::StateValidityCheckerPtr
(new DensoStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
/* set a state propagator */
setStatePropagator(oc::StatePropagatorPtr(new DensoStatePropagator(si_)));
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
void SuperBotSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
// set start, goal, and threshold
setStartAndGoalStates(start, goal, threshold);
// set propagation step size and min-max propagation steps
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// initialize a new KPIECE planner
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
// set default parameters
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
// set a state validity checker
setStateValidityChecker(ob::StateValidityCheckerPtr(new SuperBotStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
// set a state propagator
setStatePropagator(oc::StatePropagatorPtr(new SuperBotStatePropagator(si_)));
}
