astar_sim.h

关于机器人路径规划的算法实现

						if (j >= this->map_height) j = this->map_height - 1;

						if(this->Map->mapdata[i][j])
								 nearest_obstacle = radius;
					}
			//cout<<"\n R="<<radius;
			//fflush(stdout);
			}
		S->obstacle_cost =  (r - nearest_obstacle*this->pixel_size)/r; // this is a normalized cost, it will make more sense later on 
		//cout<<"\n Obstacle Cost ="<<S->obstacle_cost;
		//getchar();
		S = S->next;
		}
	this->AddText(g_strdup_printf("\n	--->>> NODES CONNECTED <<<---	"));
	cout<<"\n	--->>> NODES CONNECTED <<<---	";
	};
bool AstarPlanner::CheckShortestDistance(double x,double y,double neigbhour_distance)
	{
	SearchSpaceNode * S;
	double distance,shortest_distance = 10000000;

	S = this->search_space;
	while (S!=NULL)
		{
		distance = sqrt(pow(S->location.x - x,2)+pow(S->location.y - y,2));
		if (distance < shortest_distance)
			shortest_distance = distance;
		S = S->next;
		}
	if( shortest_distance > neigbhour_distance )
		return 1;
	else
		return 0;
	};
void AstarPlanner::ExpandObstacles() // Nifty way of doing an obstacle expansion, not finalized, ----->>>>> @ TO DO @ <<<<<------
	{
	if(this->Map->mapdata == NULL )
		{
		this->AddText(g_strdup_printf("\n	--->>> You have to read the Map before Expanding the Obstacles <<<---	"));
		return;
		}
	int thickness;
	int m,n,x,y,radius;
	thickness = int(this->obstacle_radius/this->pixel_size);
	radius =2; // This covers vertical, horizontal and diagonal cells
	for(int i=0;i<this->map_width - 1;i++)
		for(int j=0;j<this->map_height - 1 ;j++)
			{
		for(x = (i - radius) ; x <= (i + radius) ; x ++)
				{
						if (x < 0) x = 0;
						if (x >= this->map_width)  break;
						y = (int)(- sqrt(radius*radius - (x - i)*(x - i)) + j);
						if (y < 0) y = 0;
						if (y >= this->map_height) y = this->map_height - 1;
						if (this->Map->mapdata[i][j] && !this->Map->mapdata[x][y])
							{
							for (int r = 1; r <(int)(thickness);r++)
								for( m = (int)(i - r); m <= (int)(i + r);m++)
									{
										if (m < 0) m = 0;
										if (m >= this->map_width)  break;
										n = (int)(- sqrt(r*r - (m - i)*(m - i)) + j);
										if (n < 0) n = 0;
										if (n >= this->map_height) n = this->map_height - 1;
										this->Map->DrawPixel(0xff,0xff,0xff,m,n);
										n = (int)(+ sqrt(r*r - (m - i)*(m - i)) + j);
										if (n < 0) n = 0;
										if (n >= this->map_height) n = this->map_height - 1;
										this->Map->DrawPixel(0xff,0xff,0xff,m,n);
									}
							break;
							}
						y = (int)(+ sqrt(radius*radius - (x - i)*(x - i)) + j);
						if (y < 0) y = 0;
						if (y >= this->map_height) y = this->map_height - 1;
						if (this->Map->mapdata[i][j] && !this->Map->mapdata[x][y])
							{
							for (int r = 1; r <(int)(thickness);r++)
								for( m = (int)(i - r); m <= (int)(i + r);m++)
									{
										if (m < 0) m = 0;
										if (m >= this->map_width)  break;
										n = (int)(- sqrt(r*r - (m - i)*(m - i)) + j);
										if (n < 0) n = 0;
										if (n >= this->map_height) n = this->map_height - 1;
										this->Map->DrawPixel(0xff,0xff,0xff,m,n);
										n = (int)(+ sqrt(r*r - (m - i)*(m - i)) + j);
										if (n < 0) n = 0;
										if (n >= this->map_height) n = this->map_height - 1;
										this->Map->DrawPixel(0xff,0xff,0xff,m,n);
									}
							break;
							}
				}
			}
	cout<<"\n	--->>> OBSTACLES EXPANDED SUCCESSFULLY <<<---	";
	this->Map->ReadMap(); // we changed the pixel buffer, so reading the image again will regenerate the free space / MapData ;)
	this->Map->SavePixelBufferToFile(); // saving the newly generated space, default file is mapname_FreeSpace.jpeg
	};
void AstarPlanner::SaveSearchSpace()
	{
	Point p;
	temp = this->search_space;
	while (temp != NULL)
		{
		p = temp->location;
		this->ConvertToPixel(&p);
		this->Map->DrawPixel(0x00,0xff,0x00,int(p.x),(int)p.y);// Drawing the new nodes into the Pixel Buffer
		temp =temp->next;
		}
	this->Map->SavePixelBufferToFile(); // Saving the newly generated Pixel Buffer into the file
	}
void AstarPlanner::GenerateRegularGrid(double distance)
	{
	if(this->Map->mapdata == NULL )
		return;
	Point p;
	for(int i=0; i<this->map_width; i++)
		{
		for(int j=0;j<this->map_height;j++)
			{
			if (!this->Map->mapdata[i][j]) //Free Pixel
				{
				if (this->search_space == NULL ) // Constructing the ROOT NODE
					{
					temp = new SearchSpaceNode;
					temp->location.x = i;
					temp->location.y = j;
					this->ConvertPixel(&temp->location);
					temp->parent   = NULL;
					temp->next     = NULL;
					search_space = temp;
					}
				else
					{
					p.x = i;
					p.y = j;
					this->ConvertPixel(&p);
						if (CheckShortestDistance(p.x,p.y,distance))
							{
							temp = new SearchSpaceNode;
							temp->location.x = p.x;
							temp->location.y = p.y;
							temp->parent = NULL; 
							temp->next   = search_space;
							search_space = temp;
							}
					}
				}
			}
		}
	cout<<"\n	--->>> REGULAR GRID GENERATED SUCCESSFULLY <<<---	";
	this->SaveSearchSpace();
	};
void AstarPlanner::BridgeTest(double length, double neigbhour_distance)
	{
	int radius,pixels_per_bridge;
	Point p;
	double x,y,x2,y2;
	pixels_per_bridge = (int) (length/this->pixel_size);
	radius = (int) (length/(this->pixel_size*2.0));
	for(int i=0; i < this->map_width - pixels_per_bridge; i++)
		{
		for(int j=0;j<this->map_height - pixels_per_bridge ;j++)
			{
			int ab=0;
					for( x = (i - radius) ; x < (i + radius) ; x+=(radius/2.0))//x+=(radius))
						{
							// The circle is of Center (i,j) and radius R=radius
							ab++;
							if (x < 0) x = 0;
							if (x > this->map_width) break;						
							y = (int)(+ sqrt(radius*radius -  ( x - i)*( x - i)) + j);
							if (y < 0) y = 0;
							if (y >= this->map_height) y = this->map_height - 1;

							x2 = i + (i - x);
							if (x2 < 0) x2 = 0;
							if (x2 > this->map_width) break;	
							y2 = (int)(- sqrt(radius*radius - (x2 - i)*(x2 - i)) + j);
							if (y2 < 0) y2 = 0;
							if (y2 >= this->map_height) y2 = this->map_height - 1;
						//cout<<"\n x="<<x<<" y="<<y<<" x2="<<x2<<" y2="<<y2<<" i="<<i<<" j="<<j<<" R="<<radius;
						//fflush(stdout);
						if (!this->Map->mapdata[i][j]&&this->Map->mapdata[(int)x][(int)y]&&this->Map->mapdata[(int)x2][(int)y2])
							{
								p.x = i;
								p.y = j;
								this->ConvertPixel(&p);
								if (this->search_space == NULL ) // Constructing the ROOT NODE
									{
									temp = new SearchSpaceNode;
									temp->location.x = p.x;
									temp->location.y = p.y;
									temp->parent   = NULL;
									temp->next     = NULL;
									search_space = temp;
									}
								else
									{
										if (CheckShortestDistance(p.x,p.y,neigbhour_distance))
											{
											temp = new SearchSpaceNode;
											temp->location.x = p.x;
											temp->location.y = p.y;
											temp->parent = NULL;
											temp->next   = search_space;
											search_space = temp;
											}
									}
							}
						}
			//cout<<"\n I have iterated = "<<ab;
			//fflush(stdout);
			}
		}
	cout<< "\n	--->>> BRIDGE TEST FINISHED SUCCESSFULLY <<<---";
	this->SaveSearchSpace();
	};

void AstarPlanner :: ReadMap()
	{
	Map = new MapFile(this->MapFilename,pixel_size);
	this->pixbuf = Map->ReadMap();
	this->mapinfo = Map->GetMapInfo();
	this->map_width=mapinfo.width;
	this->map_height=mapinfo.height;
	this->MAXNODES=map_height*map_width;
	};
double AstarPlanner :: Absolute(double x)
	{
	if (x>=0)
		return x;
	else
		return -x;
	};
void AstarPlanner :: ConvertPixel(Point  *p) // transfers from pixel coordinate to the main coordinate system
	{
	p->x= p->x*this->pixel_size - this->pixel_size*this->map_width/2;
	p->y=-p->y*this->pixel_size + this->pixel_size*this->map_height/2;
	return ;
	};
void AstarPlanner ::ConvertToPixel (Point *p)
	{
	p->x=( p->x + this->pixel_size*this->map_width/2)/this->pixel_size;
	p->y=(-p->y + this->pixel_size*this->map_height/2)/this->pixel_size;
	}
void AstarPlanner :: PrintNodeList()
{
	int step=1;
	Point  pixel;
	if(!(p = this->path))
		return ;
	cout <<"\n  --------------------   START OF LIST ---------------------- \n";
	while(p !=NULL)
		{
		pixel =  p->NodeInfo;
		//ConvertToPixel(&pixel);
		cout << "\nStep [" << step++ <<"] corresponding to Pixel x["<< pixel.x<<"]y["<<pixel.y<<"]"; 
		for(int i=0;i<this->number_of_point_to_check;i++)
			{
			if (p->wheelchair.check_points.size() == 0)
				{
				cout<<"\n FOR some FUCKING REASON AM EMPTY :| !!!!!"; //SHOULD NEVER HAPPEN :@
				break;
				}
			}
		fflush(stdout);
		p = p->next;
		}
	cout <<"\n\n  --------------------   END OF LIST ---------------------- \n";
}
void AstarPlanner::FindRoot() // find the nearest node to the start
	{
	if(!this->search_space)
		return;
	double distance,shortest_distance = 100000;
	root = new Node;	// allocate and setup the root node
	temp = this->search_space;
	while(temp!=NULL)
		{
		distance = sqrt(pow(temp->location.x - start.x,2) + pow(temp->location.y - start.y,2));
		if (distance < shortest_distance) // Initialize the root node information and put it in the open list
			{
			shortest_distance = distance;
			root->NodeInfo.x = temp->location.x;
			root->NodeInfo.y = temp->location.y;
			}
		temp = temp->next;
		}
	root->parent = NULL;
	root->next = NULL;
	root->prev = NULL;
	root->g_value = 0;;
	root->h_value = Calculate_h(root);
	root->f_value = root->g_value + root->h_value;
	root->id = 0;
	root->depth = 0;
	root->state = OPEN;
	root->angle=this->initial_angle;
	Translate(root->NodeInfo,this->initial_angle);
	root->wheelchair.SetCheckPoints(this->number_of_point_to_check,this->points_to_check);
	cout<<"\n---->>>Root is Set to be X="<<root->NodeInfo.x<<" Y="<<root->NodeInfo.y;
	fflush(stdout);
	};
int AstarPlanner :: StartSearch(Point start_search, Point end_search,double initial_angle, double final_angle)
{
	int      gblID = 1;
  	int      gblExpand = 0;
	this->initial_angle = initial_angle;
	this->final_angle = final_angle;
	cout <<"\n	--->>> 1- Initial angle="<<RTOD(this->initial_angle)<<" Final Angle="<<RTOD(this->final_angle);
	fflush(stdout);
	if (!this->Map) // Make sure that we have a map to search
		{
		this->Map->ReadMap();
		cout <<"\nRead the Map File First !!!"<<endl;
		fflush(stdout);
		this->AddText("\nRead the Map File First !!!");
		return -1;
		}
	if (!this->search_space)
		{
		cout <<"\n	--->>> Generate Search Space First !!!"<<endl;
		fflush(stdout);
		this->AddText("\n	--->>> Generate Search Space First !!!");
		return -1;
		}
	this->ConvertPixel(&start_search);
	this->ConvertPixel(&end_search);
	this->start.x = start_search.x;
	this->start.y = start_search.y;
	this->end.x   = end_search.x;
	this->end.y   = end_search.y;
	cout <<"\n	--->>> Search Started <<<---";
	fflush(stdout);
	FindRoot();
	cout <<"\n---->>>Target is Set to be X="<<end.x<<" Y="<<end.y<<" <<<---";
	fflush(stdout);
  	openList = NULL;  // generate the open list
  	closedList = NULL;// generate the closed list
  	openList = root;
	while (openList != NULL) 
	{
    		current = openList;	                      // remove the first node from the open list, as it will always be sorted
    		openList = openList->next;
    		if(openList != NULL)
    			openList->prev = NULL;
    		gblExpand++;
    		if (GoalReached(current))                     // We reached the target location, so build the path and return it.
		{
			// build the complete path to return
      			current->next = NULL;
      			path = current;
      			p = current->parent;
    			printf("\n	--->>> Goal state reached with %d nodes created and %d nodes expanded <<<---\n", gblID, gblExpand);
			fflush(stdout);
			cout<<"\n	--->>> General Clean UP <<<---";
			fflush(stdout);
			int m=0;
      			p = current;
   			while (p != NULL) 
				{
				cout<<"\n	--->>> Step["<<++m<<"] X="<<p->NodeInfo.x<<" Y="<<p->NodeInfo.y;
				//cout<<"\n	--->>> Angle is="<<RTOD(p->angle);
				fflush(stdout);
				p = p->parent;
				}
      			p = current->parent;
   			while (p != NULL) 
				{
				//cout<<"\n Am Still HERE Step["<<++m<<"] X="<<p->NodeInfo.x<<" Y="<<p->NodeInfo.y;
				//fflush(stdout);
				if (p->prev != NULL) 			// remove the parent node from the closed list (where it has to be)
					(p->prev)->next = p->next;
				if (p->next != NULL)
					(p->next)->prev = p->prev;
				if (p == closedList)			// check if we're removing the top of the list
		  			closedList = p->next;
				p->next = path;				// set it up in the path
				path = p;
				p = p->parent;
				}
        		// now delete all nodes on OPEN
			cout<<"\n	--->>> Freeing open list <<<---";
			fflush(stdout);
			while (openList != NULL) 
				{
				p = openList->next;
				FreeNode(openList);