Exemple #1
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 = 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();
}