int main(int argc, char* argv[])
{
try
{
po::options_description desc("Options");
desc.add_options()
("help", "show help message")
("dubins", "use Dubins state space")
("dubinssym", "use symmetrized Dubins state space")
("reedsshepp", "use Reeds-Shepp state space (default)")
("easyplan", "solve easy planning problem and print path")
("hardplan", "solve hard planning problem and print path")
("trajectory", po::value<std::vector<double > >()->multitoken(),
"print trajectory from (0,0,0) to a user-specified x, y, and theta")
("distance", "print distance grid")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc,
po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
po::notify(vm);
if (vm.count("help") || argc==1)
{
std::cout << desc << "\n";
return 1;
}
ob::StateSpacePtr space(new ob::ReedsSheppStateSpace);
if (vm.count("dubins"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace);
if (vm.count("dubinssym"))
space = ob::StateSpacePtr(new ob::DubinsStateSpace(1., true));
if (vm.count("easyplan"))
plan(space, true);
if (vm.count("hardplan"))
plan(space, false);
if (vm.count("trajectory"))
printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
if (vm.count("distance"))
printDistanceGrid(space);
}
catch(std::exception& e) {
std::cerr << "error: " << e.what() << "\n";
return 1;
}
catch(...) {
std::cerr << "Exception of unknown type!\n";
}
return 0;
}
void singlePlanner::executeTrajectory(vector<Vector> &_bestTraj,
vector<Vector> &_bestTrajRoot,
const string &color)
{
updatePlanner();
_bestTraj = generateTrajectory();
_bestTrajRoot = getBestTrajRoot();
printTrajectory();
if (running_mode=="single")
{
displayPlan(color);
logTrajectory();
}
}
