astar_sim.h

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

				openList = p;
      				}
      			// now delete all nodes on CLOSED
			cout<<"\n	--->>> Freeing closed list <<<---";
			fflush(stdout);
      			while (closedList != NULL) 
				{
				p = closedList->next;
				FreeNode(closedList);
				closedList = p;
      				}
      			return 1; // Path Found Successfully
    		}
		if(!(childList = MakeChildrenNodes(current))) // No more Children => Search Ended Unsuccessfully at this path Branch
		{
			cout<<"\n	--->>> Search Ended On this Branch / We Reached a DEAD END <<<---";
			fflush(stdout);
		}
    		while (childList != NULL)                     // insert the children into the OPEN list according to their f values
		{
      			curChild  = childList;
      			childList = childList->next;
			// set up the rest of the child node details
      			curChild->parent = current;
      			curChild->state = OPEN;
      			curChild->depth = current->depth + 1;
      			curChild->id = gblID++;
      			curChild->next = NULL;
      			curChild->prev = NULL;
      			curChild->g_value = Calculate_g(curChild);
	      		curChild->h_value = Calculate_h(curChild);
      			curChild->f_value = curChild->g_value + curChild->h_value;
   			if (closedList) // test whether the child is in the closed list (already been there)
			{
				p = closedList;
				while (p) 
				{
	  			if (NodeEquality(p, curChild)) 	       		   // if so, check this child already exists in the closed list
					{
	  				if (p->f_value <= curChild->f_value)       // if the f value of the older node is lower, delete the new child
						{
							//cout<<"\n	--->>> Closed list -- Current Child deleted cause it has a longer path<<<---";
							//fflush(stdout);
			        			FreeNode(curChild);
	      						curChild = NULL;
	      						break;
	    					}
	    				else // the child is a shorter path to this point, delete p from  the closed list
						{
						// This is the tricky part, it rarely happens, but in my case it happenes all the time :s
						// Anyways, we are here cause we found a better path to a node that we already visited, we will have to
						// Update the cost of that node and ALL ITS DESCENDENTS because their cost is parent dependent ;)
						// Another Solution is simply to comment everything and do nothing, doing this, the child will be added to the
						// Open List and it will be investigated further more later on.
/*							cout<<"\n	--->>> Closed list -- Node is deleted, current child X="<<curChild->NodeInfo.x<<" Y="<<curChild->NodeInfo.y<<" has shorter path<<<---";
							fflush(stdout);
							if (p->prev != NULL)	        
					      			(p->prev)->next = p->next;
			      				if (p->next != NULL)	        
								(p->next)->prev = p->prev;								
	      						if (p == closedList)
								closedList     = p->next;
							p->f_value = curChild->f_value;
							//p->parent = curChild->parent;
							//curChild->parent = p->parent;
							q = openList;
							while(q)	// Update the DESCENDENTS with the new cost
								{
									if(NodeEquality(p,q->parent))
										{
										//q->parent = curChild;
      										q->g_value = Calculate_g(q);
	      									q->h_value = Calculate_h(q);	
      										q->f_value = q->g_value + q->h_value;
										}
								q = q->next;
								}
							bool sorted = FALSE;
							Node * tmp;
							while (!sorted) // Bubble sorting the open list
								{
									q = openList;
									while (q && q->next)
									{
										sorted = TRUE;
										if(q->next->f_value < q->f_value)
											{
												tmp = q->next;
												q->next = tmp->next;
												tmp->prev = q->prev;	
												q->prev = tmp;
												tmp->next = q;
												if(q->prev) 
													q->prev->next = tmp;
												if(tmp->next)
													tmp->next->prev = q;
												if (q == openList)
													openList = tmp;
												sorted = FALSE;
											}
									q = q->next;
									}
								}
			        			FreeNode(curChild);
	      						curChild = NULL;
	      						//FreeNode(p);*/
	      						break;
	    					}
	  				}
	  			p = p->next;
				}
      			}	
			if (curChild) 		       // check if the child is already in the open list
			{
				p = openList;
				while (p)
				{
				if (NodeEquality(p, curChild)) // child is on the OPEN list
					{
					if (p->f_value <= curChild->f_value)  // child is a longer path to the same place so delete it
						{
							//cout<<"\n	--->>> Open list -- Current Child Deleted cause it exist in the  with longer path<<<---";
							//fflush(stdout);
							FreeNode(curChild);
							curChild = NULL;
							break;
						}
					else 			              // child is a shorter path to the same place so remove the duplicate node
						{
							//cout<<"\n	--->>> Open list -- Duplicated node deleted cause current child has shorter path <<<---";
							//fflush(stdout);
							if (p->prev != NULL)
								(p->prev)->next = p->next;
							if (p->next != NULL)
								(p->next)->prev = p->prev;
							if (p == openList)
								openList = p->next;
							FreeNode(p);
							break;
						}
					}
				p = p->next;
				}
			if (curChild)
				{
					p = openList;
					q = p;
					while (p) // now insert the child into the open list according to the f value
					{
	    				if (p->f_value >= curChild->f_value) 	       // insert before p
						{
							//cout<<"\n	--->>> Child added to the Open List <<<---";
							//fflush(stdout);
	      						if (p == openList)// test head of the list case
								openList = curChild;	
		      					curChild->next = p;
	      						curChild->prev = p->prev;
	      						p->prev = curChild;
	      						if (curChild->prev)
								(curChild->prev)->next = curChild;
		      					break;
	    					}
	    				q = p;
	    				p = p->next;
	  				}		
	  				if (p == NULL)       
					{
						//cout<<"\n	--->>> Inserting Child at Start or End of the Open List<<<---";
						//fflush(stdout);
						if (q != NULL) // insert at the end
						{
				      			q->next = curChild;
		      					curChild->prev = q;
	    					}
	    					else	      // insert at the beginning
							openList = curChild;
	  				}
				}       
			} 	 		
    		} 	 	// Children Node loop end       
		// put the current node onto the closed list, ==>> already visited List
    		current->next = closedList;
    		if (closedList != NULL)
      			closedList->prev = current;
    		closedList = current;
		current->prev = NULL;
    		current->state = CLOSED;
    		if (current->id > this->MAXNODES)     	// Test to see if we have expanded too many nodes without a solution
		{
	    		cout<<"\n	--->>>	Expanded more than the maximum allowable nodes of MAXNODE="<<this->MAXNODES<<" Terminating ...";
			fflush(stdout);
			while (openList != NULL)     	// delete all nodes on OPEN
			{
				p = openList->next;
				FreeNode(openList);
				openList = p;
      			}
      			while (closedList != NULL)      // delete all nodes on CLOSED
			{
				p = closedList->next;
				FreeNode(closedList);
				closedList = p;
      			}
			return 0; // Expanded more than the maximium nodes state
    		}
// This part is for testing reasons ONLY
/*		p = openList;
		int r =0,o;
		while (p) // Printing Open List   -->> for debugging reasons
		{
			cout<<"\n--->>> ["<<++r<<"]- Open List X="<<p->NodeInfo.x<<" Y="<<p->NodeInfo.y<<" Cost ="<<p->f_value;
			q = p->parent;
			o = 0;
			while (q)
				{
				cout<<"\n	--->>> Parent ["<<++o<<"] X="<<q->NodeInfo.x<<" Y="<<q->NodeInfo.y<<" Cost ="<<q->f_value;
				//if (o >10) break;
				q = q->parent;
				if (!q) cout <<"\n	--->>>ROOT<<<---";
				}
			fflush(stdout);
			p = p->next;
		}
		p = closedList;
		r =0;
		while (p) // Printing Closed List -->> for debugging reasons
		{
			cout<<"\n--->>> ["<<++r<<"]- Closed List X="<<p->NodeInfo.x<<" Y="<<p->NodeInfo.y<<" Cost="<<p->f_value;
			fflush(stdout);
			q = p->parent;
			o = 0;
			while (q)
				{
				cout<<"\n	--->>> Parent ["<<++o<<"] X="<<q->NodeInfo.x<<" Y="<<q->NodeInfo.y<<" Cost ="<<q->f_value;
				//if (o >10) break;
				q = q->parent;
				if (!q) cout <<"\n	--->>>ROOT<<<---";
				}
			p = p->next;
		}
		getchar();*/
 	}					       // end of OPEN loop
  	// if we got here, then there is no path to the goal
  	// delete all nodes on CLOSED since OPEN is now empty
  	while (closedList != NULL) 
	{
    		p = closedList->next;
    		FreeNode(closedList);
    		closedList = p;
  	}
  	return -1; // No Path Found
};
double AstarPlanner:: Calculate_g(Node *n) // define the g function as parent plus 1 step
{
	double cost;
	if(n == NULL)
		return 0.0;
	if(n->parent == NULL)
		return 0.0;
	cost = n->parent->g_value + sqrt(pow(n->NodeInfo.x - n->parent->NodeInfo.x,2) + pow(n->NodeInfo.y - n->parent->NodeInfo.y,2) );
	return cost;
};
double AstarPlanner::Calculate_h(Node *n) //define the h function as the Euclidean distance to the goal + turning penalty
{
	double h,angle_cost,obstacle_penalty;
	if(n == NULL)
		return(1e+5);
	// Using the Euclidean distance
	h = sqrt(pow(this->end.x - n->NodeInfo.x,2) + pow(this->end.y - n->NodeInfo.y,2));
	if (n->parent != NULL) // Adding the Angle cost, we have to uniform the angle representation to the +ve rep or we well get a non sense result
		{
		if (n->parent->angle < 0 )	
			if (n->angle <0)
				angle_cost = (DTOR(360) + n->parent->angle) - (n->angle + DTOR(360)); // in Radians
			else 
				angle_cost = (DTOR(360) + n->parent->angle) - n->angle; // in Radians
		else
			if (n->angle <0)
				angle_cost = n->parent->angle - (n->angle + DTOR(360)); // in Radians
			else
				angle_cost = n->parent->angle - n->angle; // in Radians
		}
	obstacle_penalty = n->nearest_obstacle; 
	// this has a maximium value of 1 because it's normalized 
	// Now the trick is to wisely distribute the weights of the costs
	// My priority is to have a path that is smooth and goes in the middle of the free space
	// That's why i will make the obstacle cost propotional to the angle cost
	// 0.555 is the AXLE Length , we don't care which direction we are turning, it's a turn anyways
	return ( h + 0.555 * Absolute(angle_cost) + obstacle_penalty );
}

// define the goalNode function

bool AstarPlanner :: GoalReached (Node *n) 
{
	double a,b;
	if ((a = RTOD(n->angle))          < 0 ) a +=360;
	if ((b = RTOD(this->final_angle)) < 0 ) b +=360;
	if (sqrt(pow(n->NodeInfo.x - this->end.x,2)+pow(n->NodeInfo.y - this->end.y,2)) <= 0.3 && Absolute(a - b)<=60)  
		return 1;
	return 0;
};
void AstarPlanner::Translate(Point  P, double theta) // Rotates and Translates the check points according to the vehicle position and orientation
	{
	for(int i=0;i<this->number_of_point_to_check;i++)
		{
		this->points_to_check[i].x = (this->wheelchair->check_points[i].x*cos(theta) - this->wheelchair->check_points[i].y*sin(theta) + P.x);
		this->points_to_check[i].y = (this->wheelchair->check_points[i].x*sin(theta) + this->wheelchair->check_points[i].y*cos(theta) + P.y);
		//cout<<" After Conversion X="<<this->points_to_check[i].x<<" Y="<<this->points_to_check[i].y;
		}
	};
void AstarPlanner::Translate_edges(Point  P, double theta) // Rotates and Translates the check points according to the vehicle position and orientation
	{
	for(int i=0;i<4;i++)
		{
		this->translated_edge_points[i].x = (this->local_edge_points[i].x*cos(theta) - this->local_edge_points[i].y*sin(theta) + P.x);
		this->translated_edge_points[i].y = (this->local_edge_points[i].x*sin(theta) + this->local_edge_points[i].y*cos(theta) + P.y);
		cout<<" \nAfter Conversion X="<<this->translated_edge_points[i].x<<" Y="<<this->translated_edge_points[i].y;
		}
	};
// Test for whether a point is in an obstacle or not
int AstarPlanner :: Obstacle(double x, double y, double angle) 
{
	Point P,s;
	int m,n;
	P.x = x;
	P.y = y;
	Translate(P,angle); // Rotates and Translates the check points according to the vehicle position and orientation
	for (int i=0;i<this->number_of_point_to_check;i++)
	{
		this->ConvertToPixel(&this->points_to_check[i]);
		m = (int)this->points_to_check[i].x;
		n = (int)this->points_to_check[i].y;
		//cout <<"\nx ="<<m<<" y="<<n;
		//fflush(stdout);
		if (m <= 0 || n <= 0 || m >=this->map_width || n >=this->map_height)
			return 1;
		if (this->Map->mapdata[m][n]) return 1;
	};	
	return 0;
};
void AstarPlanner::draw_path()
{
  if (!this->path)
	{
		cout<<"\n->NO Path Generated Yet, plan a Path First";
		return;
	}
	GtkWidget * temp;
	double angle;
	temp = lookup_widget (GTK_WIDGET(widget),"drawingarea1");
	Point l_start,l_end,E,S;
  	Node *p;
  	p = this->path;
	while(p != NULL && p->next!=NULL)
	{
		S = l_start =  p->NodeInfo;
		ConvertToPixel(&l_start);
		E = l_end   =  p->next->NodeInfo;
		ConvertToPixel(&l_end);
  		temp = lookup_widget (widget,"drawingarea1");
		if(p->parent!=NULL)
		{
			angle = atan2(E.y - S.y,E.x - S.x); // Angle in Radians
			Translate_edges(S,angle);
			for (int i=0 ;i < 4; i++)
			{
				this->ConvertToPixel(&translated_edge_points[i]);
				cout<<"\nEdge "<< i+1<<" X="<<translated_edge_points[i].x<<" Y="<<translated_edge_points[i].y;
			}
			cout<<"\n";
 		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)translated_edge_points[0].x,(int)translated_edge_points[0].y,
		(int)translated_edge_points[1].x,(int)translated_edge_points[1].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)translated_edge_points[1].x,(int)translated_edge_points[1].y,
		(int)translated_edge_points[2].x,(int)translated_edge_points[2].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)translated_edge_points[2].x,(int)translated_edge_points[2].y,
		(int)translated_edge_points[3].x,(int)translated_edge_points[3].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)translated_edge_points[3].x,(int)translated_edge_points[3].y,
		(int)translated_edge_points[0].x,(int)translated_edge_points[0].y);

  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)l_start.x,(int)l_start.y,(int)l_end.x,(int)l_end.y);
		}
		p = p->next;
	} 
};
Node *AstarPlanner :: MakeChildrenNodes(Node *parent) 
{
	Point P; // grand parents  and parent node information
	Node  *p, *q;
	double angle,angle_difference,discrete_angle,parent_angle,child_angle;//,angle_resolution = DTOR(20);
	bool collides = FALSE;