void CObjectHandler::set_goal (MonsterSpace::EObjectAction object_action, CGameObject *game_object, u32 min_queue_size, u32 max_queue_size, u32 min_queue_interval, u32 max_queue_interval)
{
planner().set_goal(object_action,game_object,min_queue_size,max_queue_size,min_queue_interval,max_queue_interval);
}
void planWithSimpleSetupPen(std::string title = "Default")
{
// x, theta, velocity, angular veloctiy
ompl::base::StateSpacePtr space;
ompl::base::StateSpacePtr pos(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr theta(new ompl::base::SO2StateSpace());
ompl::base::StateSpacePtr vel(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr omega(new ompl::base::SO2StateSpace());
ompl::base::RealVectorBounds position_limit(1);
position_limit.setLow(-xaxis_limit);
position_limit.setHigh(xaxis_limit);
pos->as<ompl::base::RealVectorStateSpace>()->setBounds(position_limit);
ompl::base::RealVectorBounds vel_limit(1);
vel_limit.setLow(-std::numeric_limits<double>::infinity());
vel_limit.setHigh(std::numeric_limits<double>::infinity());
vel->as<ompl::base::RealVectorStateSpace>()->setBounds(vel_limit);
space = pos + theta + vel + omega;
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 1));
// set the bounds for the control space
//ob::RealVectorBounds cbounds(1);
//cbounds.setLow(0);
//cbounds.setHigh(acceleration_limit);
//cspace->as<RealVectorControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(isStateValidPen);
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &PendulumODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, PendulumPostIntegration));
/// create a start state
ob::ScopedState<> start(space);
start[0] = 0.0;
start[1] = -0.35;
start[2] = 0.0;
start[3] = 0.0;
/// create a goal state; use the hard way to set the elements
ob::ScopedState<> goal(space);
goal[1] = 0.0;
goal[3] = 0.0;
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
ss.getSpaceInformation()->setMinMaxControlDuration(1, 10);
ss.getSpaceInformation()->setPropagationStepSize(0.01);
ompl::base::PlannerPtr planner(new ompl::control::PID(ss.getSpaceInformation()));
ss.setPlanner(planner);
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10);
/*
if (solved)
{
ompl::control::PathControl& path = ss.getSolutionPath();
path.printAsMatrix(std::cout);
// print path to file
std::ofstream fout("path.txt");
path.printAsMatrix(fout);
fout.close();
}
*/
}
void CObjectHandler::update ()
{
START_PROFILE("Object Handler")
planner().update ();
STOP_PROFILE
}
int planrobarm(int argc, char *argv[])
{
int bRet = 0;
double allocated_time_secs = 5.0; //in seconds
MDPConfig MDPCfg;
//Initialize Environment (should be called before initializing anything else)
EnvironmentROBARM environment_robarm;
if(!environment_robarm.InitializeEnv(argv[1]))
{
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
//Initialize MDP Info
if(!environment_robarm.InitializeMDPCfg(&MDPCfg))
{
printf("ERROR: InitializeMDPCfg failed\n");
exit(1);
}
//plan a path
vector<int> solution_stateIDs_V;
bool bforwardsearch = false;
ARAPlanner planner(&environment_robarm, bforwardsearch);
if(planner.set_start(MDPCfg.startstateid) == 0)
{
printf("ERROR: failed to set start state\n");
exit(1);
}
if(planner.set_goal(MDPCfg.goalstateid) == 0)
{
printf("ERROR: failed to set goal state\n");
exit(1);
}
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
FILE* fSol = fopen("sol.txt", "w");
for(unsigned int i = 0; i < solution_stateIDs_V.size(); i++) {
environment_robarm.PrintState(solution_stateIDs_V[i], true, fSol);
}
fclose(fSol);
//print a path
if(bRet)
{
//print the solution
printf("Solution is found\n");
}
else
printf("Solution does not exist\n");
fflush(NULL);
return bRet;
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE3StateSpace());
// set the bounds for the R^3 part of SE(3)
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE3StateSpace>()->setBounds(bounds);
// construct an instance of space information from this state space
ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
// set state validity checking for this space
si->setStateValidityChecker(std::bind(&isStateValid, std::placeholders::_1));
// create a random start state
ob::ScopedState<> start(space);
start.random();
// create a random goal state
ob::ScopedState<> goal(space);
goal.random();
// create a problem instance
ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
// set the start and goal states
pdef->setStartAndGoalStates(start, goal);
// create a planner for the defined space
ob::PlannerPtr planner(new og::RRTConnect(si));
// set the problem we are trying to solve for the planner
planner->setProblemDefinition(pdef);
// perform setup steps for the planner
planner->setup();
// print the settings for this space
si->printSettings(std::cout);
// print the problem settings
pdef->print(std::cout);
// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = planner->solve(1.0);
if (solved)
{
// get the goal representation from the problem definition (not the same as the goal state)
// and inquire about the found path
ob::PathPtr path = pdef->getSolutionPath();
std::cout << "Found solution:" << std::endl;
// print the path to screen
path->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
int start(int argc, char *argv[])
{
QCoreApplication app(argc, argv); //Non - GUI app.
TripPlanner planner(argv[1],argv[2]);
return app.exec();
}
int planandnavigate3dkin(int argc, char *argv[])
{
double allocated_time_secs_foreachplan = 2.0; //in seconds
MDPConfig MDPCfg;
EnvironmentNAV3DKIN environment_nav3Dkin;
EnvironmentNAV3DKIN trueenvironment_nav3Dkin;
int size_x=-1,size_y=-1;
double startx = 0, starty = 0, starttheta = 0;
double goalx=-1, goaly=-1, goaltheta = -1;
double goaltol_x = 0.1, goaltol_y = 0.1, goaltol_theta = 0.1;
FILE* fSol = fopen("sol.txt", "w");
//int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
//int dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
bool bPrint = false, bPrintMap = false;
int x,y;
vector<int> preds_of_changededgesIDV;
vector<nav2dcell_t> changedcellsV;
nav2dcell_t nav2dcell;
int i;
double cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs;
vector<sbpl_2Dpt_t> perimeterptsV;
bool bsearchuntilfirstsolution = false;
unsigned char obsthresh = 0;
//srand(0);
//initialize true map and robot map
if(!trueenvironment_nav3Dkin.InitializeEnv(argv[1]))
{
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
trueenvironment_nav3Dkin.GetEnvParms(&size_x, &size_y, &startx, &starty, &starttheta, &goalx, &goaly, &goaltheta, &cellsize_m,
&nominalvel_mpersecs, &timetoturn45degsinplace_secs, &obsthresh);
unsigned char* map = (unsigned char*)calloc(size_x*size_y, sizeof(unsigned char));
//print the map
if(bPrintMap) printf("true map:\n");
for(y = 0; bPrintMap && y < size_y; y++){
for(x = 0; x < size_x; x++){
printf("%3d ", trueenvironment_nav3Dkin.GetMapCost(x,y));
}
printf("\n");
}
if(bPrint) system("pause");
printf("start: %f %f %f, goal: %f %f %f\n", startx, starty, starttheta, goalx, goaly, goaltheta);
//set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
sbpl_2Dpt_t pt_m;
double side = 0.6;
pt_m.x = -side;
pt_m.y = -side;
perimeterptsV.push_back(pt_m);
pt_m.x = side;
pt_m.y = -side;
perimeterptsV.push_back(pt_m);
pt_m.x = side;
pt_m.y = side;
perimeterptsV.push_back(pt_m);
pt_m.x = -side;
pt_m.y = side;
perimeterptsV.push_back(pt_m);
//compute sensing
double maxx = 0;
double maxy = 0;
for(i = 0; i < (int)perimeterptsV.size(); i++)
{
if(maxx < fabs(perimeterptsV.at(i).x))
maxx = fabs(perimeterptsV.at(i).x);
if(maxy < fabs(perimeterptsV.at(i).y))
maxy = fabs(perimeterptsV.at(i).y);
}
int sensingrange_c = (int)(__max(maxx, maxy)/cellsize_m) + 2;
printf("sensing range=%d cells\n", sensingrange_c);
vector<sbpl_2Dcell_t> sensecells;
for(i = -sensingrange_c; i <= sensingrange_c; i++)
{
sbpl_2Dcell_t sensecell;
sensecell.x = i;
sensecell.y = sensingrange_c;
sensecells.push_back(sensecell);
sensecell.x = i;
sensecell.y = -sensingrange_c;
sensecells.push_back(sensecell);
sensecell.x = sensingrange_c;
sensecell.y = i;
sensecells.push_back(sensecell);
sensecell.x = -sensingrange_c;
sensecell.y = i;
sensecells.push_back(sensecell);
}
//Initialize Environment (should be called before initializing anything else)
if(!environment_nav3Dkin.InitializeEnv(size_x, size_y, map, 0,0,0,0,0,0,
goaltol_x, goaltol_y, goaltol_theta, perimeterptsV,
cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs,
obsthresh)){
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
/*
if(!environment_nav3Dkin.InitializeEnv(size_x, size_y, map, startx, starty, starttheta, goalx, goaly, goaltheta,
goaltol_x, goaltol_y, goaltol_theta, perimeterptsV,
cellsize_m, nominalvel_mpersecs, timetoturn45degsinplace_secs,
obsthresh)){
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
*/
environment_nav3Dkin.SetStart(startx, starty,starttheta);
//Initialize MDP Info
if(!environment_nav3Dkin.InitializeMDPCfg(&MDPCfg))
{
printf("ERROR: InitializeMDPCfg failed\n");
exit(1);
}
//create a planner
vector<int> solution_stateIDs_V;
bool bforwardsearch = false;
//ARAPlanner planner(&environment_nav3Dkin, bforwardsearch);
ADPlanner planner(&environment_nav3Dkin, bforwardsearch);
planner.set_initialsolution_eps(5.0);
//set search mode
planner.set_search_mode(bsearchuntilfirstsolution);
//set the start and goal configurations
if(planner.set_start(MDPCfg.startstateid) == 0)
{
printf("ERROR: failed to set start state\n");
exit(1);
}
MDPCfg.goalstateid = environment_nav3Dkin.SetGoal(goalx, goaly, goaltheta);
if(planner.set_goal(MDPCfg.goalstateid) == 0)
{
printf("ERROR: failed to set goal state\n");
exit(1);
}
//now comes the main loop
int goalx_c = CONTXY2DISC(goalx, cellsize_m);
int goaly_c = CONTXY2DISC(goaly, cellsize_m);
int goaltheta_c = ContTheta2Disc(goaltheta, NAV3DKIN_THETADIRS);
printf("goal_c: %d %d %d\n", goalx_c, goaly_c, goaltheta_c);
while(fabs(startx - goalx) > goaltol_x || fabs(starty - goaly) > goaltol_y || fabs(starttheta - goaltheta) > goaltol_theta){
//simulate sensor data update
bool bChanges = false;
preds_of_changededgesIDV.clear();
changedcellsV.clear();
for(i = 0; i < (int)sensecells.size(); i++){
int x = CONTXY2DISC(startx,cellsize_m) + sensecells.at(i).x;
int y = CONTXY2DISC(starty,cellsize_m) + sensecells.at(i).y;
if(x < 0 || x >= size_x || y < 0 || y >= size_y)
continue;
int index = x + y*size_x;
unsigned char truecost = trueenvironment_nav3Dkin.GetMapCost(x,y);
if(map[index] != truecost){
map[index] = truecost;
environment_nav3Dkin.UpdateCost(x,y,map[index]);
printf("setting cost[%d][%d] to %d\n", x,y,map[index]);
bChanges = true;
//store the changed cells
nav2dcell.x = x;
nav2dcell.y = y;
changedcellsV.push_back(nav2dcell);
}
}
double TimeStarted = clock();
if(bChanges){
//planner.costs_changed(); //use by ARA* planner (non-incremental)
//get the affected states
environment_nav3Dkin.GetPredsofChangedEdges(&changedcellsV, &preds_of_changededgesIDV);
//let know the incremental planner about them
planner.update_preds_of_changededges(&preds_of_changededgesIDV); //use by AD* planner (incremental)
//printf("%d states were affected\n", preds_of_changededgesIDV.size());
}
int startx_c = CONTXY2DISC(startx,cellsize_m);
int starty_c = CONTXY2DISC(starty,cellsize_m);
int starttheta_c = ContTheta2Disc(starttheta, NAV3DKIN_THETADIRS);
fprintf(fSol, "%d %d %d ", startx_c, starty_c, starttheta_c);
//plan a path
bool bPlanExists = false;
printf("new planning...\n");
bPlanExists = (planner.replan(allocated_time_secs_foreachplan, &solution_stateIDs_V) == 1);
printf("done with the solution of size=%d and sol. eps=%f\n", solution_stateIDs_V.size(), planner.get_solution_eps());
environment_nav3Dkin.PrintTimeStat(stdout);
fprintf(fSol, "%.5f %.2f\n", (clock()-TimeStarted)/((double)CLOCKS_PER_SEC), planner.get_solution_eps());
fflush(fSol);
//for(unsigned int i = 0; i < solution_stateIDs_V.size(); i++) {
//environment_nav3Dkin.PrintState(solution_stateIDs_V[i], true, fSol);
//}
//fprintf(fSol, "*********\n");
//print the map
int startindex = startx_c + starty_c*size_x;
int goalindex = goalx_c + goaly_c*size_x;
for(y = 0; bPrintMap && y < size_y; y++){
for(x = 0; x < size_x; x++){
int index = x + y*size_x;
//check to see if it is on the path
bool bOnthePath = false;
for(int j = 1; j < (int)solution_stateIDs_V.size(); j++)
{
int newx, newy, newtheta=0;
environment_nav3Dkin.GetCoordFromState(solution_stateIDs_V[j], newx, newy, newtheta);
if(x == newx && y == newy)
bOnthePath = true;
}
if (index != startindex && index != goalindex && !bOnthePath)
printf("%3d ", map[index]);
else if(index == startindex)
printf(" X ");
else if(index == goalindex)
printf(" G ");
else if (bOnthePath)
printf(" * ");
else
printf("? ");
}
printf("\n");
}
//move along the path
if(bPlanExists && (int)solution_stateIDs_V.size() > 1){
//get coord of the successor
int newx, newy, newtheta;
environment_nav3Dkin.GetCoordFromState(solution_stateIDs_V[1], newx, newy, newtheta);
printf("moving from %d %d %d to %d %d %d\n", startx_c, starty_c, starttheta_c, newx, newy, newtheta);
//this check is weak since true configuration does not know the actual perimeter of the robot
if(!trueenvironment_nav3Dkin.IsValidConfiguration(newx,newy,newtheta))
{
printf("ERROR: robot is commanded to move into an invalid configuration\n");
exit(1);
}
if(!environment_nav3Dkin.IsValidConfiguration(newx,newy,newtheta))
{
printf("ERROR: robot is commanded to move into an invalid configuration\n");
exit(1);
}
//move
startx = DISCXY2CONT(newx, cellsize_m);
starty = DISCXY2CONT(newy, cellsize_m);
starttheta = DiscTheta2Cont(newtheta, NAV3DKIN_THETADIRS);
//update the environment
environment_nav3Dkin.SetStart(startx, starty,starttheta);
//update the planner
if(planner.set_start(solution_stateIDs_V[1]) == 0){
printf("ERROR: failed to update robot pose in the planner\n");
exit(1);
}
}
else
{
printf("No move is made\n");
}
if(bPrint) system("pause");
}
printf("goal reached!\n");
fflush(NULL);
return 1;
}
int plan3dkin(int argc, char *argv[])
{
int bRet = 0;
double allocated_time_secs = 3.0; //in seconds
MDPConfig MDPCfg;
//set the perimeter of the robot (it is given with 0,0,0 robot ref. point for which planning is done)
vector<sbpl_2Dpt_t> perimeterptsV;
sbpl_2Dpt_t pt_m;
double halfwidth = 0.3;
double halflength = 0.45;
pt_m.x = -halflength;
pt_m.y = -halfwidth;
perimeterptsV.push_back(pt_m);
pt_m.x = halflength;
pt_m.y = -halfwidth;
perimeterptsV.push_back(pt_m);
pt_m.x = halflength;
pt_m.y = halfwidth;
perimeterptsV.push_back(pt_m);
pt_m.x = -halflength;
pt_m.y = halfwidth;
perimeterptsV.push_back(pt_m);
//Initialize Environment (should be called before initializing anything else)
EnvironmentNAV3DKIN environment_nav3Dkin;
if(!environment_nav3Dkin.InitializeEnv(argv[1], perimeterptsV))
{
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
//Initialize MDP Info
if(!environment_nav3Dkin.InitializeMDPCfg(&MDPCfg))
{
printf("ERROR: InitializeMDPCfg failed\n");
exit(1);
}
//plan a path
vector<int> solution_stateIDs_V;
bool bforwardsearch = false;
ADPlanner planner(&environment_nav3Dkin, bforwardsearch);
if(planner.set_start(MDPCfg.startstateid) == 0)
{
printf("ERROR: failed to set start state\n");
exit(1);
}
if(planner.set_goal(MDPCfg.goalstateid) == 0)
{
printf("ERROR: failed to set goal state\n");
exit(1);
}
planner.set_initialsolution_eps(4.0);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_nav3Dkin.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_nav3Dkin.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_nav3Dkin.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_nav3Dkin.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_nav3Dkin.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
FILE* fSol = fopen("sol.txt", "w");
for(unsigned int i = 0; i < solution_stateIDs_V.size(); i++) {
environment_nav3Dkin.PrintState(solution_stateIDs_V[i], false, fSol);
}
fclose(fSol);
environment_nav3Dkin.PrintTimeStat(stdout);
//print a path
if(bRet)
{
//print the solution
printf("Solution is found\n");
}
else
printf("Solution does not exist\n");
fflush(NULL);
return bRet;
}
int planxythetalat(int argc, char *argv[])
{
int bRet = 0;
double allocated_time_secs = 0.5; //in seconds
MDPConfig MDPCfg;
//read in motion primitives
FILE* fmprimitives = fopen("xytheta_mprimitives_1.cfg", "r");
if(fmprimitives == NULL)
{
printf("ERROR: motion primitives file can not be opened\n");
exit(1);
}
vector<SBPL_xytheta_mprimitive> mprimV;
while(!feof(fmprimitives))
{
SBPL_xytheta_mprimitive mprim;
mprim.intermptV.clear();
float ftemp;
fscanf(fmprimitives, "%f", &ftemp);
mprim.endx_m = ftemp;
fscanf(fmprimitives, "%f", &ftemp);
mprim.endy_m = ftemp;
fscanf(fmprimitives, "%f", &ftemp);
mprim.endtheta_rad = ftemp;
mprimV.push_back(mprim);
}
//Initialize Environment (should be called before initializing anything else)
EnvironmentNAVXYTHETALAT environment_navxythetalat;
if(!environment_navxythetalat.InitializeEnv(argv[1], &mprimV))
{
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
//Initialize MDP Info
if(!environment_navxythetalat.InitializeMDPCfg(&MDPCfg))
{
printf("ERROR: InitializeMDPCfg failed\n");
exit(1);
}
//plan a path
vector<int> solution_stateIDs_V;
bool bforwardsearch = false;
ADPlanner planner(&environment_navxythetalat, bforwardsearch);
if(planner.set_start(MDPCfg.startstateid) == 0)
{
printf("ERROR: failed to set start state\n");
exit(1);
}
if(planner.set_goal(MDPCfg.goalstateid) == 0)
{
printf("ERROR: failed to set goal state\n");
exit(1);
}
planner.set_initialsolution_eps(4.0);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_navxythetalat.PrintTimeStat(stdout);
/*
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_navxythetalat.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_navxythetalat.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_navxythetalat.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
environment_navxythetalat.PrintTimeStat(stdout);
printf("start planning...\n");
bRet = planner.replan(allocated_time_secs, &solution_stateIDs_V);
printf("done planning\n");
std::cout << "size of solution=" << solution_stateIDs_V.size() << std::endl;
*/
FILE* fSol = fopen("sol.txt", "w");
for(unsigned int i = 0; i < solution_stateIDs_V.size(); i++) {
environment_navxythetalat.PrintState(solution_stateIDs_V[i], false, fSol);
}
fclose(fSol);
environment_navxythetalat.PrintTimeStat(stdout);
//print a path
if(bRet)
{
//print the solution
printf("Solution is found\n");
}
else
printf("Solution does not exist\n");
fflush(NULL);
return bRet;
}
// Call setup functions
bool GlobalPlanner::Init(ros::NodeHandle* nh)
{
ROS_INFO_STREAM("Initializing global planner class");
m_nh = nh;
SetupCallbacks();
ROS_INFO_STREAM("Initializing TaskMaster");
std::string waypointFile("testList1.points");
if (m_nh->getParam("/global_planner/waypoints_file", waypointFile))
{
ROS_INFO_STREAM("Read from file: "<<waypointFile);
}
// Planner types :
// naive : original, for each waypoint in order of file, choose first available robot
// closest_robot : for each waypoint in order of file, choose closest available robot
// closest_waypoint : for each available robot in order of id, choose closest available waypoint
std::string planner("naive");
m_planner = PLANNER_NAIVE;
if (m_nh->getParam("/global_planner/planner", planner))
{
ROS_INFO_STREAM("Planner set from file to " << planner);
if (planner.compare("closest_robot") == 0) {
m_planner = PLANNER_CLOSEST_ROBOT;
}
else if (planner.compare("closest_waypoint") == 0) {
m_planner = PLANNER_CLOSEST_WAYPOINT;
}
}
// Load list of possible dumpsites
// std::string dumpsiteFile("cubicle_dump_meets.points");
std::string dumpsiteFile("springdemo_dump_meets.points");
m_nh->getParam("/global_planner/dumpsite_file", dumpsiteFile);
ROS_INFO_STREAM("Loading dumpsite file: " << dumpsiteFile);
loadDumpSites(dumpsiteFile);
// Need to wait for m_robots to get populated by callback
// Some better ways todo: call globalplanner with param saying # of robots, then wait till # robots is populated
// for now just pausing for 10 seconds
// Unsustainable. Must hack harder.
// ROS_INFO_STREAM("Waiting till 2 robot controllers are loaded and update global planner robot map...");
// int robot_count = 0;
// while (robot_count < 2)
// {
// robot_count = 0;
// for (std::map<int, Robot_Ptr>::iterator it = m_robots.begin(); it != m_robots.end(); ++it)
// {
// robot_count++;
// }
// ROS_INFO_STREAM(robot_count << " robots loaded so far.");
// ros::spinOnce();
// sleep(1);
// }
// ROS_INFO("Done waiting, initializing task master.");
m_tm.Init(nh, m_robots, waypointFile);
ros::spinOnce();
return true;
}
void single_robot_planner_example(int argc, char* argv[])
{
//typedefs
typedef mms::Mms_path_planner_example<> Planner;
typedef Motion_sequence_gui_converter<Planner::K> Converter;
//loading files from configuration file
Time_manager tm;
tm.write_time_log(std::string("start"));
Environment<> env(argc,argv);
tm.write_time_log(std::string("set environment"));
//loading scene from environment
Planner::Polygon_vec& workspace(env.get_workspace());
Planner::Extended_polygon my_robot(env.get_robot_a());
//initialize the planner and preprocess
Planner planner(workspace, my_robot);
planner.preprocess();
//load query
Planner::Reference_point q_s (env.get_source_configuration_a());
Planner::Reference_point q_t (env.get_target_configurations().front());
//perform query
Motion_sequence<Planner::K> motion_sequence;
bool found_path = planner.query(q_s, q_t, motion_sequence);
if (!found_path)
std::cout<<"no path found :-("<<std::endl;
else
std::cout<<"path found :-)"<<std::endl;
//example of how to create a path that can be loaded by the GUI
if (found_path)
{
Converter::Path_3d path;
Converter converter;
converter(motion_sequence, std::back_inserter(path));
ofstream os("path.txt");
os <<path.size()<<std::endl;
for (unsigned int i(0); i < path.size(); ++i)
{
if (i == 0)
{
os << path[i].x <<" " << path[i].y <<" " << path[i].t <<" "
<< CGAL::to_double(q_t.get_location().x()) <<" " //we place the second robot at the target for ease of visualization
<< CGAL::to_double(q_t.get_location().y()) <<" " //we place the second robot at the target for ease of visualization
<< q_t.get_rotation().to_angle() //we place the second robot at the target for ease of visualization
<<std::endl;
}
else //i >0
{
os << path[i].x <<" " << path[i].y <<" " << path[i].t <<" " //first robot location
<< 0 <<" " << 0 <<" " << 0 <<" " //second robot is static
<< 2 //speed is arbitrarily chosen to be 2
<<std::endl;
}
}
}
return;
}
/*
* PerformCursorOpen
* Execute SQL DECLARE CURSOR command.
*/
void
PerformCursorOpen(DeclareCursorStmt *stmt)
{
List *rewritten;
Query *query;
Plan *plan;
Portal portal;
MemoryContext oldContext;
/*
* Disallow empty-string cursor name (conflicts with protocol-level
* unnamed portal).
*/
if (!stmt->portalname || stmt->portalname[0] == '\0')
ereport(ERROR,
(errcode(ERRCODE_INVALID_CURSOR_NAME),
errmsg("invalid cursor name: must not be empty")));
/*
* If this is a non-holdable cursor, we require that this statement
* has been executed inside a transaction block (or else, it would
* have no user-visible effect).
*/
if (!(stmt->options & CURSOR_OPT_HOLD))
RequireTransactionChain((void *) stmt, "DECLARE CURSOR");
/*
* Because the planner is not cool about not scribbling on its input,
* we make a preliminary copy of the source querytree. This prevents
* problems in the case that the DECLARE CURSOR is in a portal and is
* executed repeatedly. XXX the planner really shouldn't modify its
* input ... FIXME someday.
*/
query = copyObject(stmt->query);
/*
* The query has been through parse analysis, but not rewriting or
* planning as yet. Note that the grammar ensured we have a SELECT
* query, so we are not expecting rule rewriting to do anything
* strange.
*/
rewritten = QueryRewrite(query);
if (length(rewritten) != 1 || !IsA(lfirst(rewritten), Query))
elog(ERROR, "unexpected rewrite result");
query = (Query *) lfirst(rewritten);
if (query->commandType != CMD_SELECT)
elog(ERROR, "unexpected rewrite result");
if (query->into)
ereport(ERROR,
(errcode(ERRCODE_SYNTAX_ERROR),
errmsg("DECLARE CURSOR may not specify INTO")));
if (query->rowMarks != NIL)
ereport(ERROR,
(errcode(ERRCODE_FEATURE_NOT_SUPPORTED),
errmsg("DECLARE CURSOR ... FOR UPDATE is not supported"),
errdetail("Cursors must be READ ONLY.")));
plan = planner(query, true, stmt->options);
/*
* Create a portal and copy the query and plan into its memory
* context.
*/
portal = CreatePortal(stmt->portalname, false, false);
oldContext = MemoryContextSwitchTo(PortalGetHeapMemory(portal));
query = copyObject(query);
plan = copyObject(plan);
PortalDefineQuery(portal,
NULL, /* unfortunately don't have sourceText */
"SELECT", /* cursor's query is always a SELECT */
makeList1(query),
makeList1(plan),
PortalGetHeapMemory(portal));
MemoryContextSwitchTo(oldContext);
/*
* Set up options for portal.
*
* If the user didn't specify a SCROLL type, allow or disallow scrolling
* based on whether it would require any additional runtime overhead
* to do so.
*/
portal->cursorOptions = stmt->options;
if (!(portal->cursorOptions & (CURSOR_OPT_SCROLL | CURSOR_OPT_NO_SCROLL)))
{
if (ExecSupportsBackwardScan(plan))
portal->cursorOptions |= CURSOR_OPT_SCROLL;
else
portal->cursorOptions |= CURSOR_OPT_NO_SCROLL;
}
/*
* Start execution --- never any params for a cursor.
*/
PortalStart(portal, NULL);
Assert(portal->strategy == PORTAL_ONE_SELECT);
/*
* We're done; the query won't actually be run until
* PerformPortalFetch is called.
*/
}
int main(int argc, char* argv[])
{
//init Pathplanner
cout << "start planner ... " << endl;
std::tr1::function<float(float, float, float)> callback;
callback = &computeVelo;
SILIA_Pathplanner planner(callback);
vector<float> vec1; //<! start configuration
vector<float> vec2; //<! goal configuration
vector< pair<vector<float>, bool> > path; //<! target path, consists of start and goal configuration
/*
vec1.push_back(90);
vec1.push_back(30);
vec1.push_back(30);
vec1.push_back(0);
vec1.push_back(0);
vec1.push_back(0);
vec2.push_back(0);
vec2.push_back(90);
vec2.push_back(-90);
vec2.push_back(90);
vec2.push_back(90);
vec2.push_back(90);
path.push_back(pair<vector<float>, bool> (vec1, true));
path.push_back(pair<vector<float>, bool> (vec2, true));
*/
vector<float> a2,b2;
a2.push_back(-26.7);
a2.push_back(-72.4);
a2.push_back(78.7);
a2.push_back(-0.5);
a2.push_back(82.3);
a2.push_back(17.9);
/*b2.push_back(35.1);
b2.push_back(-75.1);
b2.push_back(84.3);
b2.push_back(0.1);
b2.push_back(79.3);
b2.push_back(-3.6);
*/
b2.push_back(-39.1);
b2.push_back(-45.1);
b2.push_back(34.3);
b2.push_back(-0.1);
b2.push_back(97.3);
b2.push_back(-3.8);
path.push_back(pair<vector<float>, bool> (a2, true));
path.push_back(pair<vector<float>, bool> (b2, true));
vector<pair<vector<float>,float>> solution = planner.computePath(path); //<! solution path consists of start, goal and intermediate configurations
cout << "Path: " << endl;
for(unsigned int i=0; i<solution.size(); i++)
{
for(unsigned int k=0; k<solution[i].first.size(); k++)
{
cout << solution[i].first[k] << " ";
}
cout << "|| Velocity: " << solution[i].second;
cout << endl;
}
LogManager * _logManager;
_logManager = new LogManager(NULL);
KukaEthernetClient client;
cout << "start client..." << endl;
getchar();
client.Initialize("EKIServerFrame.xml", SocketAddress("192.1.10.20", 49152));
//client.Initialize("TestKRL.xml", SocketAddress("localhost", 9978));
client.setCallbackFcn(testCallback);
cout << "start sending messages..." << endl;
getchar();
//client.movePTP(currentMsgID, KukaAxis(-33, -113, 100, 0, 100, 11), 10);// oben
for(unsigned int i=0; i<solution.size(); i++)
{
currentMsgID = client.getMessageID();
client.movePTP(currentMsgID, KukaAxis(solution[i].first), solution[i].second * 100);// oben
}
//float axSpeed_base = 10;
//for (float i = 1; i <= 10; i++)
//{
// currentMsgID = client.getMessageID();
// client.movePTP(currentMsgID, KukaAxis(-33, -113, 100, 0, 100, 11), axSpeed);// oben
// //client.movePTP(client.getMessageID(), KukaAxis(-53, -114, 110, 0, 92,-9), axSpeed);// mitte rechts
// //client.movePTP(client.getMessageID(), KukaAxis(-33, -113, 118, 0, 83, 11), axSpeed);//unten
// //client.movePTP(client.getMessageID(), KukaAxis(-20, -109, 107, 0, 90, 24), axSpeed);// mitte links
//}
//getchar();
////cout << "Press to close..." << endl;
getchar();
return 0;
}
void display()
{
//std::cout << "In case of path not found, please set Arm arg range to -PI to PI\n";
N2D::render::clean_screen();
ArmRobot<DIM>::CONFIG start( {M_PI/2 - .01, -M_PI/2 +0.4, 1.4} );
ArmRobot<DIM>::CONFIG goal( {M_PI - .81, -1.01, 0.31} );
//ArmRobot<DIM>::CONFIG goal( {M_PI - .1, 0.15, 0.1} );
// Set up the world
//std::vector<N2D::Polygon> polies = parse_file("/Users/Yinan/workspace/RSS2014/C++/RSS2015/ji.txt", N2D::v2(M_PI, M_PI));
std::vector<N2D::Polygon> polies = parse_file("/Users/IanZhang/Documents/workspace/RSS2015/C++/RSS2015/ji.txt", N2D::v2(M_PI, M_PI));
//std::vector<N2D::Polygon> polies = parse_file("/Users/yuhanlyu/Documents/RSS2015/C++/RSS2015/ji.txt", geometry::v2(M_PI, M_PI));
std::vector<robotics::Obstacle> obsts;
/*
for (N2D::Polygon poly : polies) {
obsts.emplace_back(poly);
}*/
// Simple World
std::vector<N2D::v2> p1_points( { N2D::v2( 0.5, 1.2 ), N2D::v2(0.8, 1.2), N2D::v2(0.8, 0.9), N2D::v2( 0.5, 0.9) } );
std::vector<N2D::v2> p2_points( { N2D::v2( 1.4, 1.6 ), N2D::v2(1.7, 1.6), N2D::v2(1.7, 1.3), N2D::v2( 1.4, 1.3) } );
std::vector<N2D::v2> p3_points( { N2D::v2( 2.2, 0.7 ), N2D::v2(2.5, 0.7), N2D::v2(2.5, 0.4), N2D::v2( 2.2, 0.4) } );
N2D::Polygon p1(p1_points);
N2D::Polygon p2(p2_points);
N2D::Polygon p3(p3_points);
obsts.push_back(p1);
obsts.push_back(p2);
obsts.push_back(p3);
robotics::ObstManager obstacle_manager(obsts);
if( planning_cells.size() == 0 && !render_workspace )
{
// Decompose C-Space
algorithms::KDDecomposer<ArmRobot<DIM>> sampler(robot, obstacle_manager, epsilon);
std::clock_t t0 = std::clock();
//sampler.DecomposeSpace();
sampler.AdaptiveDecompose(M_PI/64, M_PI/2048);
std::clock_t t1 = std::clock();
std::cout << "Time cost for decomposing C-space:\n\t" << (t1-t0) / (double)(CLOCKS_PER_SEC / 1000) << "ms\n";
printf("Free cells: %d\n", (int)sampler.get_free_cells().size());
planning_cells = sampler.get_free_cells();
// Start Planning
// Set up planner
algorithms::Planner<ArmRobot<DIM>> planner( sampler );
planner.initialize(sampler, goal, robot);
// Search for path
std::clock_t t4 = std::clock();
path = planner.AStar(robot, start, goal);
std::clock_t t5 = std::clock();
std::cout << "Time cost for finding the shortest path:\n\t" << (t5-t4) / (double)(CLOCKS_PER_SEC / 1000) << "ms\n";
planning_cells = planner.get_cells();
}
// Render Cells
if( render_cells && !render_workspace )
{
const double max_weight = 0;//*std::max_element(std::begin(importance), std::end(importance));
const double min_weight = 0;//*std::min_element(std::begin(importance), std::end(importance));
int count = 0;
for (int i = 0; i < planning_cells.size(); i++)
{
ND::sphere<DIM> temp_sphere = planning_cells[i].sphere();
N2D::sphere temp2d_sphere( N2D::v2( temp_sphere.center()[0], temp_sphere.center()[1] ), temp_sphere.radius(), N2D::SPHEREMETRIC::LINFTY );
if( planning_cells[i].visited() )
{
N2D::render::sphere(temp2d_sphere, N2D::render::Color( 0,220,0, 20 ), true);
N2D::render::sphere(temp2d_sphere, N2D::render::Color( 0,250,0, 250 ), false);
}
else
{
N2D::render::sphere(temp2d_sphere, N2D::render::Color( 50,50,50, 40 ), false);
/*
if( temp2d_sphere.radius() <= M_PI/64)
{
count ++;
double weight = importance[i] / ( max_weight/8.0 - min_weight );
N2D::render::sphere(temp2d_sphere, N2D::render::Color( 255*weight,0,0, 40 ), true);
}
*/
}
}
std::cout << count << '\n';
}
// Render obstacles
if(render_workspace)
{
const std::vector<robotics::Obstacle> obsts = obstacle_manager.get_obsts();
for (robotics::Obstacle obst : obsts)
{
N2D::Polygon poly( obst.vertices );
N2D::render::polygon( poly, N2D::render::Color(100, 100, 100, 100));
}
robot.set_config(start);
const std::vector<N2D::Polygon> shapes = robot.get_shape();
for (N2D::Polygon shape : shapes)
{
N2D::render::polygon( shape, N2D::render::Color(250, 0, 0), false);
}
robot.set_config(goal);
const std::vector<N2D::Polygon> shapes2 = robot.get_shape();
for (N2D::Polygon shape : shapes2)
{
N2D::render::polygon( shape, N2D::render::Color(250, 0, 0), false);
}
}
if(path.size() != 0 && render_workspace)
{
render_robot(path[0], 250, 0, 0);
for( int i = 1; i < path.size(); i++ )
{
render_robot(path[i],135, 206, 250, std::max( (int)(float(i)/float(path.size()) * 60), 10));
}
}
render_robot(start,250, 0, 0);
render_robot(goal,0, 0, 250);
N2D::render::flush();
}
bool CObjectHandler::goal_reached ()
{
return (planner().solution().size() < 2);
}
int planandnavigate2d(int argc, char *argv[])
{
double allocated_time_secs_foreachplan = 10.0; //in seconds
MDPConfig MDPCfg;
EnvironmentNAV2D environment_nav2D;
EnvironmentNAV2D trueenvironment_nav2D;
int size_x=-1,size_y=-1;
int startx = 0, starty = 0;
int goalx=-1, goaly=-1;
FILE* fSol = fopen("sol.txt", "w");
int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1};
int dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1};
bool bPrint = false;
int x,y;
vector<int> preds_of_changededgesIDV;
vector<nav2dcell_t> changedcellsV;
nav2dcell_t nav2dcell;
int i;
unsigned char obsthresh = 0;
//srand(0);
int plantime_over1secs=0, plantime_over0p5secs=0, plantime_over0p1secs=0, plantime_over0p05secs=0, plantime_below0p05secs=0;
//initialize true map and robot map
if(!trueenvironment_nav2D.InitializeEnv(argv[1]))
{
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
trueenvironment_nav2D.GetEnvParms(&size_x, &size_y, &startx, &starty, &goalx, &goaly, &obsthresh);
unsigned char* map = (unsigned char*)calloc(size_x*size_y, sizeof(unsigned char));
//print the map
if(bPrint) printf("true map:\n");
for(y = 0; bPrint && y < size_y; y++){
for(x = 0; x < size_x; x++){
printf("%d ", (int)trueenvironment_nav2D.IsObstacle(x,y));
}
printf("\n");
}
if(bPrint) system("pause");
//Initialize Environment (should be called before initializing anything else)
if(!environment_nav2D.InitializeEnv(size_x, size_y, map, startx, starty, goalx, goaly, obsthresh)){
printf("ERROR: InitializeEnv failed\n");
exit(1);
}
//Initialize MDP Info
if(!environment_nav2D.InitializeMDPCfg(&MDPCfg))
{
printf("ERROR: InitializeMDPCfg failed\n");
exit(1);
}
//create a planner
vector<int> solution_stateIDs_V;
bool bforwardsearch = false;
//ARAPlanner planner(&environment_nav2D, bforwardsearch);
ADPlanner planner(&environment_nav2D, bforwardsearch);
planner.set_initialsolution_eps(2.0);
//set the start and goal configurations
if(planner.set_start(MDPCfg.startstateid) == 0)
{
printf("ERROR: failed to set start state\n");
exit(1);
}
if(planner.set_goal(MDPCfg.goalstateid) == 0)
{
printf("ERROR: failed to set goal state\n");
exit(1);
}
//now comes the main loop
int goalthresh = 0;
while(abs(startx - goalx) > goalthresh || abs(starty - goaly) > goalthresh){
//simulate sensor data update
bool bChanges = false;
preds_of_changededgesIDV.clear();
changedcellsV.clear();
for(i = 0; i < 8; i++){
int x = startx + dx[i];
int y = starty + dy[i];
if(x < 0 || x >= size_x || y < 0 || y >= size_y)
continue;
int index = x + y*size_x;
unsigned char truecost = trueenvironment_nav2D.GetMapCost(x,y);
if(map[index] != truecost){
map[index] = truecost;
environment_nav2D.UpdateCost(x,y,map[index]);
printf("setting cost[%d][%d] to %d\n", x,y,map[index]);
bChanges = true;
//store the changed cells
nav2dcell.x = x;
nav2dcell.y = y;
changedcellsV.push_back(nav2dcell);
}
}
double TimeStarted = clock();
if(bChanges){
//planner.costs_changed(); //use by ARA* planner (non-incremental)
//get the affected states
environment_nav2D.GetPredsofChangedEdges(&changedcellsV, &preds_of_changededgesIDV);
//let know the incremental planner about them
planner.update_preds_of_changededges(&preds_of_changededgesIDV); //use by AD* planner (incremental)
}
fprintf(fSol, "%d %d ", startx, starty);
//plan a path
bool bPlanExists = false;
while(bPlanExists == false){
printf("new planning...\n");
bPlanExists = (planner.replan(allocated_time_secs_foreachplan, &solution_stateIDs_V) == 1);
printf("done with the solution of size=%d\n", solution_stateIDs_V.size());
environment_nav2D.PrintTimeStat(stdout);
//for(unsigned int i = 0; i < solution_stateIDs_V.size(); i++) {
//environment_nav2D.PrintState(solution_stateIDs_V[i], true, fSol);
//}
//fprintf(fSol, "*********\n");
}
double plantime_secs = (clock()-TimeStarted)/((double)CLOCKS_PER_SEC);
fprintf(fSol, "%.5f %.5f\n", plantime_secs, planner.get_solution_eps());
fflush(fSol);
if(plantime_secs > 1.0)
plantime_over1secs++;
else if(plantime_secs > 0.5)
plantime_over0p5secs++;
else if(plantime_secs > 0.1)
plantime_over0p1secs++;
else if(plantime_secs > 0.05)
plantime_over0p05secs++;
else
plantime_below0p05secs++;
//print the map
int startindex = startx + starty*size_x;
int goalindex = goalx + goaly*size_x;
for(y = 0; bPrint && y < size_y; y++){
for(x = 0; x < size_x; x++){
int index = x + y*size_x;
//check to see if it is on the path
bool bOnthePath = false;
for(int j = 1; j < (int)solution_stateIDs_V.size(); j++)
{
int newx, newy;
environment_nav2D.GetCoordFromState(solution_stateIDs_V[j], newx, newy);
if(x == newx && y == newy)
bOnthePath = true;
}
if (index != startindex && index != goalindex && !bOnthePath)
printf("%3d ", map[index]);
else if(index == startindex)
printf(" R ");
else if(index == goalindex)
printf(" G ");
else if (bOnthePath)
printf(" * ");
else
printf(" ? ");
}
printf("\n");
}
if(bPrint) system("pause");
//move along the path
if(bPlanExists && (int)solution_stateIDs_V.size() > 1){
//get coord of the successor
int newx, newy;
environment_nav2D.GetCoordFromState(solution_stateIDs_V[1], newx, newy);
if(trueenvironment_nav2D.GetMapCost(newx,newy) >= obsthresh)
{
printf("ERROR: robot is commanded to move into an obstacle\n");
exit(1);
}
//move
printf("moving from %d %d to %d %d\n", startx, starty, newx, newy);
startx = newx;
starty = newy;
//update the environment
environment_nav2D.SetStart(startx, starty);
//update the planner
if(planner.set_start(solution_stateIDs_V[1]) == 0){
printf("ERROR: failed to update robot pose in the planner\n");
exit(1);
}
}
}
//print stats
printf("stats: plantimes over 1 secs=%d; over 0.5 secs=%d; over 0.1 secs=%d; over 0.05 secs=%d; below 0.05 secs=%d\n",
plantime_over1secs, plantime_over0p5secs, plantime_over0p1secs, plantime_over0p05secs, plantime_below0p05secs);
fprintf(fSol, "stats: plantimes over 1 secs=%d; over 0.5; secs=%d; over 0.1 secs=%d; over 0.05 secs=%d; below 0.05 secs=%d\n",
plantime_over1secs, plantime_over0p5secs, plantime_over0p1secs, plantime_over0p05secs, plantime_below0p05secs);
fflush(NULL);
return 1;
}
IC void CObjectHandler::switch_torch (CInventoryItem *inventory_item, bool value)
{
CTorch *torch = smart_cast<CTorch*>(inventory_item);
if (torch && attached(torch) && planner().object().g_Alive())
torch->Switch (value);
}
bool SBPLInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::GetMotionPlan::Request &req,
moveit_msgs::GetMotionPlan::Response &res,
const PlanningParameters& params) const
{
res.trajectory.joint_trajectory.points.clear();
(const_cast<SBPLInterface*>(this))->last_planning_statistics_ = PlanningStatistics();
planning_models::RobotState *start_state(planning_scene->getCurrentState());
planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
ros::WallTime wt = ros::WallTime::now();
boost::shared_ptr<EnvironmentChain3D> env_chain(new EnvironmentChain3D(planning_scene));
if(!env_chain->setupForMotionPlan(planning_scene,
req,
res,
params)) {
//std::cerr << "Env chain setup failing" << std::endl;
return false;
}
//std::cerr << "Creation with params " << params.use_bfs_ << " took " << (ros::WallTime::now()-wt).toSec() << std::endl;
boost::this_thread::interruption_point();
//DummyEnvironment* dummy_env = new DummyEnvironment();
boost::shared_ptr<ARAPlanner> planner(new ARAPlanner(env_chain.get(), true));
planner->set_initialsolution_eps(100.0);
planner->set_search_mode(true);
planner->force_planning_from_scratch();
planner->set_start(env_chain->getPlanningData().start_hash_entry_->stateID);
planner->set_goal(env_chain->getPlanningData().goal_hash_entry_->stateID);
//std::cerr << "Creation took " << (ros::WallTime::now()-wt) << std::endl;
std::vector<int> solution_state_ids;
int solution_cost;
wt = ros::WallTime::now();
//CALLGRIND_START_INSTRUMENTATION;
bool b_ret = planner->replan(10.0, &solution_state_ids, &solution_cost);
//CALLGRIND_STOP_INSTRUMENTATION;
double el = (ros::WallTime::now()-wt).toSec();
std::cerr << "B ret is " << b_ret << " planning time " << el << std::endl;
std::cerr << "Expansions " << env_chain->getPlanningStatistics().total_expansions_
<< " average time " << (env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
<< " hz " << 1.0/(env_chain->getPlanningStatistics().total_expansion_time_.toSec()/(env_chain->getPlanningStatistics().total_expansions_*1.0))
<< std::endl;
std::cerr << "Total coll checks " << env_chain->getPlanningStatistics().coll_checks_ << " hz " << 1.0/(env_chain->getPlanningStatistics().total_coll_check_time_.toSec()/(env_chain->getPlanningStatistics().coll_checks_*1.0)) << std::endl;
std::cerr << "Path length is " << solution_state_ids.size() << std::endl;
if(!b_ret) {
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
if(solution_state_ids.size() == 0) {
std::cerr << "Success but no path" << std::endl;
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
if(!env_chain->populateTrajectoryFromStateIDSequence(solution_state_ids,
res.trajectory.joint_trajectory)) {
std::cerr << "Success but path bad" << std::endl;
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
ros::WallTime pre_short = ros::WallTime::now();
//std::cerr << "Num traj points before " << res.trajectory.joint_trajectory.points.size() << std::endl;
trajectory_msgs::JointTrajectory traj = res.trajectory.joint_trajectory;
env_chain->attemptShortcut(traj, res.trajectory.joint_trajectory);
//std::cerr << "Num traj points after " << res.trajectory.joint_trajectory.points.size() << std::endl;
//std::cerr << "Time " << (ros::WallTime::now()-pre_short).toSec() << std::endl;
//env_chain->getPlaneBFSMarker(mark, env_chain->getGoalPose().translation().z());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
PlanningStatistics stats = env_chain->getPlanningStatistics();
stats.total_planning_time_ = ros::WallDuration(el);
(const_cast<SBPLInterface*>(this))->last_planning_statistics_ = stats;
return true;
}
