astar.h

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

		}
		double m = i + 2*this->obstacle_radius,n=(l + startx);
		//cout<<"\n	m ="<<m<<" n="<<n;
		if (  m >= n  ) 
		{
			i += (l + startx - i)/2;
			//cout<<"\n It happened i="<<i;
			//fflush(stdout);
		}
		else 
			i += (2*this->obstacle_radius);
	}
	cout<<"\n	Last index is="<<point_index;
	this->wheelchair = new Robot();
	this->wheelchair->SetCheckPoints(this->number_of_point_to_check,this->points_to_check);
	for (int k=0;k<this->number_of_point_to_check;k++)
	{
		cout << "\nRobot Edge '"<<k<<"'---> X="<<this->wheelchair->check_points[k].x<<" Y="<<this->wheelchair->check_points[k].y;
		fflush(stdout);
	}
	};
AstarPlanner :: AstarPlanner(Point start, Point point,double initial_angle,double final_an,double pixel_size,double radius,GtkWidget *w,const char * MapFilename)
	{
	this->ConvertPixel(&start);
	this->ConvertPixel(&end);
	this->start.x = start.x;
	this->start.y = start.y;
	this->end.x = end.x;
	this->end.y = end.y;
	this->MapFilename = MapFilename;
	this->initial_angle= initial_angle;
	this->final_angle = final_an;
	this->pixel_size = pixel_size;
	this->obstacle_radius = radius;
	path=p=root=test=NULL;
	this->search_space=this->temp = NULL;
	this->widget = w;
	this->simulate = FALSE;
	this->DetermineCheckPoints();// Determine the Points that should be checked for obstacle avoidance on the wheelchair
	this->openList   = new LList;
	this->closedList = new LList;
	};
AstarPlanner :: ~AstarPlanner()
	{
	delete wheelchair;
	delete [] points_to_check;
	delete Map;
	path=p=root=test=NULL;
	while (this->search_space != NULL)
		{
		temp = this->search_space;
		this->search_space = this->search_space->next;
		delete temp;
		};
	this->FreePath();
	this->AddText("\n	--->>> Allocated Memory FREED <<<---");
	};
void AstarPlanner::AddText ( char const * text)
	{
	GtkWidget * view;
	GtkTextBuffer *text_buffer;
	GtkTextIter     startv, endv;
	view = lookup_widget (GTK_WIDGET(widget),"textview1");
  	text_buffer = gtk_text_view_get_buffer (GTK_TEXT_VIEW (view));
	gtk_text_buffer_insert_at_cursor(text_buffer,text,-1);
    gtk_text_buffer_get_bounds(text_buffer, &startv, &endv);
    gtk_text_view_scroll_to_iter(GTK_TEXT_VIEW(view), &endv, 0.0, FALSE, 0.0,0.0);
	}
void AstarPlanner::ShowConnections()
	{
	Point loc1,loc2;
	GtkWidget * view;
	view = lookup_widget (GTK_WIDGET(widget),"drawingarea1");
	temp = this->search_space;
	int m=0,n=0;
	while (temp != NULL)
		{
		for (unsigned int i=0; i < temp->children.size();i++)
			{
			loc1 = temp->location;
			ConvertToPixel(&loc1);
			loc2 = temp->children[i]->location;
			ConvertToPixel(&loc2);
			gdk_draw_line(view->window,(GdkGC*)(view)->style->white_gc,(int)loc1.x,(int)loc1.y,(int)loc2.x,(int)loc2.y);
			m++;
			}
		temp = temp->next;
		n++;
		}
	cout<<"\n---->>> TOTAL NUMBER OF CONNECTIONS ="<<m<<"\n---->>> Total Nodes in search Space ="<<n;
	fflush(stdout);
	this->MAXNODES = m;
	}
void AstarPlanner::ConnectNodes(double allowed_distance)
	{
	SearchSpaceNode * S;
	double distance,angle;
	if (!this->search_space) // Do nothing if Search Space is not Yet Created
		return;
	temp = this->search_space;
	while (temp!=NULL)
	{
		S = this->search_space;
		while (S!=NULL)
		{
			distance = sqrt(pow(S->location.x - temp->location.x,2) + pow(S->location.y - temp->location.y,2));
			if (distance <= allowed_distance && distance !=0)
			{
				angle = atan2(S->location.y - temp->location.y ,S->location.x - temp->location.x);
				if(!Obstacle(temp->location,angle))
					temp->children.push_back(S);
			}
			S = S->next;
		}
		temp = temp->next;
	}
	this->AddText(g_strdup_printf("\n	--->>> NODES CONNECTED <<<---	"));
	cout<<"\n	--->>> NODES CONNECTED <<<---	";
	};
void AstarPlanner::AddCostToNodes(double r)
	{
	SearchSpaceNode * S;
	Point  point;
	double number_of_pixels,radius,nearest_obstacle;
	int i,j;
	//int n=0;
	if (!this->search_space) // Do nothing if Search Space is not Yet Created
		return;
	S = this->search_space;
	number_of_pixels = r / this->pixel_size;
	while (S!=NULL)
	{
		//cout<<"\n Node= "<<++n;
		//fflush(stdout);
		point.x = S->location.x;
		point.y = S->location.y;
		this->ConvertToPixel(&point);
		nearest_obstacle = 0;
		for(radius = (int)number_of_pixels ; radius >0 ; radius --)
			{
				for( i= (int)(point.x - radius) ; i < (int)(point.x + radius); i+=5)
					{
						if (i < 0) i = 0;
						if (i >= this->map_width)  break;
						j = (int)(- sqrt(radius*radius - (i - point.x)*(i - point.x)) + point.y);
						if (j < 0) j = 0;
						if (j >= this->map_height) j = this->map_height - 1;
						if(this->Map->mapdata[i][j])
								 nearest_obstacle = radius;
						j = (int)(+ sqrt(radius*radius - (i - point.x)*(i - point.x)) + point.y);
						if (j < 0) j = 0;
						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/4.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;
	};
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;
};
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->location;
		cout <<"\nStep [" << step++ <<"] x["<< pixel.x<<"]y["<<pixel.y<<"]"<<" Direction="<<p->direction; 
		cout <<"\tG cost="<<p->g_value<<"\tH cost="<<p->h_value<<"\tFcost="<<p->f_value;
		cout<<"\tStored Angle = "<<RTOD(p->angle);
		if (p->next !=NULL)
		{
			cout<<"\tNext Angle = "<<RTOD(atan2(p->next->location.y - p->location.y, p->next->location.x - p->location.x));
			cout<<"\tAngle Diff ="<<RTOD(anglediff(p->angle,atan2(p->next->location.y - p->location.y, p->next->location.x - p->location.x)));
		}
		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;