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; }