int main(int argc, char *argv[])
{
// Profiling observation: Using int instead of double cost provides marginal improvement (~10%)
GenericSearchGraphDescriptor<myNode,double> myGraph;
// We describe the graph, cost function, heuristics, and the start & goal in this block
// ------------------------------------------------------------------------------------
// Set the functions
myGraph.getHashBin_fp = &getHashBin;
myGraph.isAccessible_fp = &isAccessible;
myGraph.getSuccessors_fp = &getSuccessors;
myGraph.getHeuristics_fp = &getHeuristics;
// Set other variables
myGraph.hashTableSize = 25; // Since in this problem, "getHashBin" can return a max of value 201.
myNode tempNode;
tempNode.x = (int)(0.8*MIN_X); tempNode.y = (int)(0.8*MIN_Y); // Start node
myGraph.SeedNode = tempNode;
tempNode.x = (int)(0.8*MAX_X); tempNode.y = (int)(0.8*MAX_Y); // Goal node
myGraph.TargetNode = tempNode;
// ------------------------------------------------------------------------------------
// Initiate visualization
CvSize map_size;
cvNamedWindow("D3N2_MovingObstacleIn2D", CV_WINDOW_AUTOSIZE);
map_size.width = PLOT_SCALE * (1 + MAX_X - MIN_X);
map_size.height = PLOT_SCALE * (1 + MAX_Y - MIN_Y);
ipl_image_p = cvCreateImage(map_size, IPL_DEPTH_8U, 3);
ipl_image_p->origin = 1;
ipl_image_p->widthStep = ipl_image_p->width * 3;
cvSet(ipl_image_p, CV_RGB(255.0,255.0,255.0));
for (int a=0; a<OBS_RECT_COUNT; a++)
cvRectangle(ipl_image_p, _cv_plot_coord(OBS_RECT[a][0],OBS_RECT[a][1]), _cv_plot_coord(OBS_RECT[a][2],OBS_RECT[a][3]),
cvScalar(200.0,200.0,200.0), -1 );
cvCircle(ipl_image_p, _cv_plot_coord(myGraph.TargetNode.x,myGraph.TargetNode.y), 0.3*PLOT_SCALE, cvScalar(0, 255.0, 0), -1);
cvShowImage("D3N2_MovingObstacleIn2D", ipl_image_p);
// Planning
A_star_planner<myNode,double> planner;
planner.setParams(A_star_eps, 10); // optional.
planner.event_NodeExpanded_nm = &myNode::event_NodeExpanded; // optional event handler.
planner.event_SuccUpdated_nm = &myNode::event_SuccUpdated; // optional event handler.
planner.init(myGraph);
planner.plan();
// Display result
std::vector< std::vector< myNode > > paths = planner.getPlannedPaths();
for (int a=0; a<paths[0].size()-1; a++)
cvLine(ipl_image_p, _cv_plot_coord(paths[0][a].x, paths[0][a].y), _cv_plot_coord(paths[0][a+1].x, paths[0][a+1].y),
cvScalar(100.0,250.0,200.0), 4);
cvShowImage("D3N2_MovingObstacleIn2D", ipl_image_p);
#if SAVE_IMG
char imgFname[1024];
sprintf(imgFname, "outfiles/%c%03d.png", imgPrefix, frameno++);
cvSaveImage(imgFname, ipl_image_p);
#endif
cvWaitKey();
}
SNode AStarStrategyOnYAGSBPL::searchPath(SNode& start, SNode& finish) {
GenericSearchGraphDescriptor<SNode, double> graph;
graph.getHashBin_fp = &getHashBin;
//graph.getHeuristics_fp = &getHeuristics;
graph.getSuccessors_fp = &getSuccessors;
graph.isAccessible_fp = &isAccessible;
graph.hashTableSize = (int)std::max(aStarStaticMap->sizeOnXaxis(), aStarStaticMap->sizeOnYaxis()) + 1;
graph.SeedNode = start;
graph.TargetNode = finish;
A_star_planner<SNode, double> planner;
planner.init(graph);
planner.plan();
std::vector< std::vector< SNode > > paths = planner.getPlannedPaths();
/*
printf("Number of paths: %lu\nPath coordinates: \n[ ", paths.size());
if (paths.size() > 0) {
for (int a=0; a<paths[0].size(); a++) {
printf("[%d, %d]{%d}; ", paths[0][a].position.first+1, paths[0][a].position.second+1, paths[0][a].direction);
}
printf(" ]\n\n");
}
*/
if (paths.size() > 0) {
int pathSize = (int)paths[0].size();
return paths[0][pathSize-2];
} else {
std::cout << "!!!NO PATH FOUND!!!" << std::endl;
return SNode(-1,-1,0,0);
}
}
int main(int argc, char *argv[])
{
// Profiling observation: Using int instead of double cost provides marginal improvement (~10%)
GenericSearchGraphDescriptor<myNode,double> myGraph;
// We describe the graph, cost function, heuristics, and the start & goal in this block
// ------------------------------------------------------------------------------------
// Set the functions
myGraph.getHashBin_fp = &getHashBin;
myGraph.isAccessible_fp = &isAccessible;
myGraph.getSuccessors_fp = &getSuccessors;
myGraph.getHeuristics_fp = &getHeuristics;
// Set other variables
myGraph.hashTableSize = 212; // Since in this problem, "getHashBin" can return a max of value 201.
myGraph.hashBinSizeIncreaseStep = 512; // By default it's 128. For this problem, we choose a higher value.
myNode tempNode;
tempNode.x = -150; tempNode.y = -150; // Start node
myGraph.SeedNode = tempNode;
tempNode.x = 150; tempNode.y = 150; // Goal node
myGraph.TargetNode = tempNode;
// ------------------------------------------------------------------------------------
// Planning
A_star_planner<myNode,double> planner;
planner.setParams(1.0, 10); // optional.
planner.init(myGraph);
planner.plan();
std::vector< std::vector< myNode > > paths = planner.getPlannedPaths();
printf("\nNumber of paths: %d\nPath coordinates: \n[ ", paths.size());
for (int a=0; a<paths[0].size(); a++)
printf("[%d, %d]; ", paths[0][a].x, paths[0][a].y);
printf(" ]\n\n");
tempNode.x = -100; tempNode.y = -100;
printf("Testing 'getNodeInfo': g-value of (%d,%d): %f\n\n", tempNode.x, tempNode.y, planner.getNodeInfo(tempNode).g);
}
int main(int argc, char *argv[])
{
// Some pre-computations
for (int a=0; a<OBS_RECT_COUNT; a++) {
OBS_CENTERS[a][0] = (OBS_RECT[a][0] + OBS_RECT[a][2]) / 2;
OBS_CENTERS[a][1] = (OBS_RECT[a][1] + OBS_RECT[a][3]) / 2;
}
STORED_PATH_COUNT = 0;
// -----------------------------------------------------------------------------------
// Profiling observation: Using int instead of double cost provides marginal improvement (~10%)
GenericSearchGraphDescriptor<myNode,double> myGraph;
// We describe the graph, cost function, heuristics, and the start & goal in this block
// ------------------------------------------------------------------------------------
// Set the functions
myGraph.getHashBin_fp = &getHashBin;
myGraph.isAccessible_fp = &isAccessible;
myGraph.getSuccessors_fp = &getSuccessors;
myGraph.getHeuristics_fp = &getHeuristics;
myGraph.storePath_fp = &storePath;
myGraph.stopSearch_fp = &stopSearch;
// Set other variables
myGraph.hashTableSize = MAX_X - MIN_X + 1;
myNode tempNode;
tempNode.x = START_COORD[0]; tempNode.y = START_COORD[1]; // Start node
myGraph.SeedNode = tempNode;
tempNode.x = GOAL_COORD[0]; tempNode.y = GOAL_COORD[1]; // Goal node
myGraph.TargetNode = tempNode;
// ------------------------------------------------------------------------------------
// Initiate visualization
CvSize map_size;
cvNamedWindow("display", CV_WINDOW_AUTOSIZE);
map_size.width = PLOT_SCALE * (1 + MAX_X - MIN_X);
map_size.height = PLOT_SCALE * (1 + MAX_Y - MIN_Y);
ipl_image_p = cvCreateImage(map_size, IPL_DEPTH_8U, 3);
ipl_image_p->origin = 1;
ipl_image_p->widthStep = ipl_image_p->width * 3;
/* cvSet(ipl_image_p, CV_RGB(255.0,255.0,255.0));
for (int a=0; a<OBS_RECT_COUNT; a++)
cvRectangle(ipl_image_p, _cv_plot_coord(OBS_RECT[a][0],OBS_RECT[a][1]), _cv_plot_coord(OBS_RECT[a][2],OBS_RECT[a][3]),
cvScalar(200.0,200.0,200.0), -1 );
cvCircle(ipl_image_p, _cv_plot_coord(myGraph.TargetNode.x,myGraph.TargetNode.y), 0.3*PLOT_SCALE, cvScalar(0, 255.0, 0), -1);
cvShowImage("display", ipl_image_p); */
// Planning
A_star_planner<myNode,double> planner;
planner.setParams(A_star_eps, 10); // optional.
//planner.event_NodeExpanded_nm = &myNode::event_NodeExpanded; // optional event handler.
//planner.event_SuccUpdated_nm = &myNode::event_SuccUpdated; // optional event handler.
planner.init(myGraph);
planner.plan();
// Display result
std::vector< myNode > goals = planner.getGoalNodes();
std::vector< std::vector< myNode > > paths = planner.getPlannedPaths();
printf("\nNumber of paths: %d\n", paths.size());
for (int p=0; p<paths.size(); p++)
{
cvSet(ipl_image_p, CV_RGB(255.0,255.0,255.0));
for (int a=0; a<OBS_RECT_COUNT; a++)
{
cvRectangle(ipl_image_p, _cv_plot_coord(OBS_RECT[a][0],OBS_RECT[a][1]), _cv_plot_coord(OBS_RECT[a][2],OBS_RECT[a][3]),
cvScalar(200.0,200.0,200.0), -1 );
cvLine(ipl_image_p, _cv_plot_coord(OBS_CENTERS[a][0], OBS_CENTERS[a][1]), _cv_plot_coord(OBS_CENTERS[a][0],MAX_Y), cvScalar(0.0,180.0,0.0), 1);
}
cvCircle(ipl_image_p, _cv_plot_coord(myGraph.TargetNode.x,myGraph.TargetNode.y), 0.3*PLOT_SCALE, cvScalar(0, 255.0, 0), -1);
printf("\tPath %d: ", p);
for (int a=0; a<goals[p].h.size(); a++)
printf(" %d ", goals[p].h[a]);
printf("\n");
double r=(double)(rand()%200), g=(double)(rand()%200), b=(double)(rand()%200);
for (int a=0; a<paths[p].size()-1; a++)
cvLine(ipl_image_p, _cv_plot_coord(paths[p][a].x, paths[p][a].y), _cv_plot_coord(paths[p][a+1].x, paths[p][a+1].y),
cvScalar(r,g,b), 2);
#if SAVE_IMG
char imgFname[1024];
sprintf(imgFname, "outfiles/%s%03d.png", imgPrefix.c_str(), p);
cvSaveImage(imgFname, ipl_image_p);
#endif
cvShowImage("display", ipl_image_p);
cvWaitKey();
}
cvWaitKey();
}
