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;
}
bool PlanningEnvironmentMPH::readinMotionPrimitive(SBPL_xytheta_mprimitive* pMotPrim, FILE* fIn, double resolutionCorrectionFactor)
{
char sTemp[1024];
int dTemp;
char sExpected[1024];
int numofIntermPoses;
//read in actionID
strcpy(sExpected, "primID:");
if (fscanf(fIn, "%s", sTemp) == 0)
return false;
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
return false;
}
if (fscanf(fIn, "%d", &pMotPrim->motprimID) != 1)
return false;
//read in start angle
strcpy(sExpected, "startangle_c:");
if (fscanf(fIn, "%s", sTemp) == 0)
return false;
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
return false;
}
if (fscanf(fIn, "%d", &dTemp) == 0)
{
SBPL_ERROR("ERROR reading start angle\n");
return false;
}
pMotPrim->starttheta_c = dTemp;
//read in end pose
strcpy(sExpected, "endpose_c:");
if (fscanf(fIn, "%s", sTemp) == 0)
return false;
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
return false;
}
if (readinCell(&pMotPrim->endcell, fIn) == false)
{
SBPL_ERROR("ERROR: failed to read in end search pose\n");
return false;
}
//read in action cost
strcpy(sExpected, "additionalactioncostmult:");
if (fscanf(fIn, "%s", sTemp) == 0)
return false;
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
return false;
}
if (fscanf(fIn, "%d", &dTemp) != 1)
return false;
pMotPrim->additionalactioncostmult = dTemp;
//read in intermediate poses
strcpy(sExpected, "intermediateposes:");
if (fscanf(fIn, "%s", sTemp) == 0)
return false;
if (strcmp(sTemp, sExpected) != 0)
{
SBPL_ERROR("ERROR: expected %s but got %s\n", sExpected, sTemp);
return false;
}
if (fscanf(fIn, "%d", &numofIntermPoses) != 1)
return false;
//all intermposes should be with respect to 0,0 as starting pose since it will be added later and should be done
//after the action is rotated by initial orientation
for (int i = 0; i < numofIntermPoses; i++)
{
sbpl_xy_theta_pt_t intermpose;
if (readinPose(&intermpose, fIn, resolutionCorrectionFactor) == false)
{
SBPL_ERROR("ERROR: failed to read in intermediate poses\n");
return false;
}
pMotPrim->intermptV.push_back(intermpose);
}
//check that the last pose corresponds correctly to the last pose
pMotPrim->endcell.x *= params::motionPrimitiveScaleFactor;
pMotPrim->endcell.y *= params::motionPrimitiveScaleFactor;
sbpl_xy_theta_pt_t sourcepose;
sourcepose.x = DISCXY2CONT(0, params::motionPrimitiveResolution);
sourcepose.y = DISCXY2CONT(0, params::motionPrimitiveResolution);
sourcepose.theta = DiscTheta2Cont(pMotPrim->starttheta_c, State::cellSize.yaw_count);
double mp_endx_m = sourcepose.x + pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].x;
double mp_endy_m = sourcepose.y + pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].y;
double mp_endtheta_rad = pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].theta;
int endx_c = CONTXY2DISC(mp_endx_m, params::motionPrimitiveResolution);
int endy_c = CONTXY2DISC(mp_endy_m, params::motionPrimitiveResolution);
int endtheta_c = ContTheta2Disc(mp_endtheta_rad, State::cellSize.yaw_count);
// sbpl_xy_theta_pt_t& pose = pMotPrim->intermptV.back();
// ROS_INFO("mp: [%0.3f %0.3f %0.3f] [%d %d %d]", pose.x, pose.y, pose.theta, pMotPrim->endcell.x, pMotPrim->endcell.y, pMotPrim->endcell.theta);
if (endx_c != pMotPrim->endcell.x || endy_c != pMotPrim->endcell.y || endtheta_c != pMotPrim->endcell.theta)
{
SBPL_ERROR("ERROR: incorrect primitive %d with startangle=%d last interm point %f %f %f does not match end pose %d %d %d\n", pMotPrim->motprimID, pMotPrim->starttheta_c, pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].x, pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].y,
pMotPrim->intermptV[pMotPrim->intermptV.size() - 1].theta, pMotPrim->endcell.x, pMotPrim->endcell.y, pMotPrim->endcell.theta);
return false;
}
pMotPrim->endcell.x /= params::motionPrimitiveScaleFactor;
pMotPrim->endcell.y /= params::motionPrimitiveScaleFactor;
return true;
}