astar.h

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

	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));
		// Initialize the root node information and put it in the open list
		if (distance < shortest_distance) 
		{
			shortest_distance = distance;
			root->location.x = temp->location.x;
			root->location.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->angle=this->initial_angle;
	root->direction = FORWARD;
	Translate(root->location,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->location.x<<" Y="<<root->location.y;
	fflush(stdout);
	};
int AstarPlanner :: StartSearch(Point start_search, Point end_search,double initial_angle, double final_angle)
{
	int      ID = 1;
  	int      NodesExpanded = 0;
	this->initial_angle = initial_angle;
	this->final_angle   = final_angle;
	if(this->tree.size() > 0)		this->tree.clear();
	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->Add(root);				// Add the root to OpenList
	cout <<"\n	--->>> Root Added <<<---"; fflush(stdout);
//  while openList is not empty 
	while (openList->Start != NULL) 
	{
		current = openList->GetHead(); 	// Get the node with the cheapest cost, first node
		openList->Next();				// Move to the next Node
    	NodesExpanded++;
    	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;
    		cout<<"\n	--->>> Goal state reached with :"<<ID<<" nodes created and :"<<NodesExpanded<<" nodes expanded <<<---\n";
			fflush(stdout);
			cout<<"\n	--->>> General Clean UP <<<---";
			fflush(stdout);
//			int m=0;
      		p = current;
//   			while (p != NULL) 
//			{
//				//cout<<"\n	--->>> Step["<<++m<<"] X="<<p->location.x<<" Y="<<p->location.y;
//				//cout<<"\n	--->>> Angle is="<<RTOD(p->angle);
//				fflush(stdout);
//				p = p->parent;
//			}
      		// Going up to the Root
      		p = current->parent;
   			while (p != NULL)
			{
//				cout<<"\n Am Still HERE Step["<<++m<<"] X="<<p->location.x<<" Y="<<p->location.y;
//				fflush(stdout);
				// remove the parent node from the closed list (where it has to be)
				if(p->prev != NULL)
					(p->prev)->next = p->next;
				if(p->next != NULL)
					(p->next)->prev = p->prev;
				// check if we're removing the top of the list
				if(p == closedList->Start)
		  			closedList->Next();
				// set it up in the path
				p->next = path;				
				path = p;
				p = p->parent;
			}
        	// now delete all nodes on OPEN and Closed Lists
			cout<<"\n	--->>> Freeing open list <<<---";   fflush(stdout);
			openList->Free();
			cout<<"\n	--->>> DONE  <<<---";	fflush(stdout);
			cout<<"\n	--->>> Freeing closed list <<<---";	fflush(stdout);
			closedList->Free();
			cout<<"\n	--->>> DONE  <<<---";	fflush(stdout);
      		return 1; 	// Path Found Successfully
    	}
    	
    	// Create List of Children for the current NODE
		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);
		}
		// insert the children into the OPEN list according to their f values
    	while (childList != NULL)                     
		{
  		    curChild  = childList;
  			childList = childList->next;
		    // set up the rest of the child node details
  			curChild->parent = current;
  			curChild->depth = current->depth + 1;
  			curChild->id = ID++;
  			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;
			Node * p;
			// check if the child is already in the open list
			if( (p = openList->Find(curChild)))
			{
  				if (p->f_value <= curChild->f_value && (p->direction == curChild->direction))       
				{
	        		FreeNode(curChild);
  					curChild = NULL;
				}
				// the child is a shorter path to this point, delete p from  the closed list
				else 
  				if (p->f_value > curChild->f_value && (p->direction == curChild->direction))
				{
					openList->Remove(p);
				 	//cout<<"\n	--->>> Opened list -- Node is deleted, current child X="<<curChild->location.x<<" Y="<<curChild->location.y<<" has shorter path<<<---";
					fflush(stdout);

				}					
			}
			// test whether the child is in the closed list (already been there)			
			if (curChild)
			if((p = closedList->Find(curChild)))
			{
  				if (p->f_value <= curChild->f_value && p->direction == curChild->direction)       
				{
	        		FreeNode(curChild);
  					curChild = NULL;
				}
				// the child is a shorter path to this point, delete p from  the closed list
				else 
				{
					/* 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 later on.
					 */
					//closedList->Remove(p);
				 	//cout<<"\n	--->>> Closed list -- Node is deleted, current child X="<<curChild->location.x<<" Y="<<curChild->location.y<<" has shorter path<<<---";
					fflush(stdout);

				}					
			}
			// ADD the child to the OPEN List
			if (curChild)
			{
				openList->Add(curChild);
			}	 		
    	}
		// put the current node onto the closed list, ==>> already visited List
		closedList->Add(current);
		// Test to see if we have expanded too many nodes without a solution
    	if (current->id > this->MAXNODES*2)     	
		{
	    	cout<<"\n	--->>>	Expanded more than the maximum allowable nodes of MAXNODE="<<this->MAXNODES<<" Terminating ...";
			fflush(stdout);
			//Delete Nodes in Open and Closed Lists
			closedList->Free();
			openList->Free();
			return 0; // Expanded more than the maximium nodes state
    	}
 	}					       // 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
	closedList->Free();
  	cout<<"\n	--->>>No Path Found<<<---";
  	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 || n->parent==NULL)
		return 0.0;
	cost = n->parent->g_value + sqrt(pow(n->location.x - n->parent->location.x,2) + pow(n->location.y - n->parent->location.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=0,angle_cost=0,obstacle_penalty=0,reverse_penalty=0,delta_d=0;
	if(n == NULL)
		return(0);
	// Using the Euclidean distance
	h = sqrt(pow(this->end.x - n->location.x,2) + pow(this->end.y - n->location.y,2));
	//h = 0;
	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
	{
		angle_cost = anglediff(n->angle,n->parent->angle); // in radians
		delta_d = sqrt(pow(n->location.x - n->parent->location.x,2) + pow(n->location.y - n->parent->location.y,2) );
	}
	obstacle_penalty = n->nearest_obstacle;
	if(n->direction == BACKWARD) 
		reverse_penalty = 3;
	else
		reverse_penalty = 0;
	// 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 * angle_cost + obstacle_penalty*delta_d + reverse_penalty );
};
// define the goalNode function

bool AstarPlanner :: GoalReached (Node *n) 
{
	double angle_diff, delta_d;
	delta_d = sqrt(pow(n->location.x - this->end.x,2)+pow(n->location.y - this->end.y,2));
	if (n->direction == FORWARD)
		angle_diff =	anglediff(this->final_angle,n->angle);
	else
	{
		angle_diff =	anglediff(this->final_angle,n->angle + M_PI);
	}
	if ( delta_d <= 0.3  && angle_diff <= DTOR(30))
	{
		cout<<" \n Desired Final Orientation ="<<RTOD(this->final_angle)<<" Current="<<RTOD(n->angle);
		cout<<"\n Reached Destination with Diff Orientation="<< RTOD(angle_diff);
		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(Point P, double angle) 
{
	Point s;
	int m,n;
	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_tree()
{
	GtkWidget * temp;
	temp = lookup_widget (GTK_WIDGET(widget),"drawingarea1");
	Point l_start,l_end;
	for (unsigned int i =0; i<tree.size();i++)
		{
			l_start = tree[i].location;
			ConvertToPixel(&l_start);
			//cout<<"\n Main X="<<tree[i].location.x<<" Y="<<tree[i].location.y;
			for(unsigned int j=0;j<tree[i].children.size();j++)
			{
				l_end = tree[i].children[j];
				ConvertToPixel(&l_end);
				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);
				//cout<<"\n	--->>>Child X="<<tree[i].children[j].x<<" Y="<<tree[i].children[j].y;
			}
		}
}
void AstarPlanner::draw_path()
{
	if (!this->path)
	{
		cout<<"\n->NO Path Generated Yet, plan a Path First";
		return;
	}
	double angle;
	GtkWidget * temp;
	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 =  p->location;
  		temp = lookup_widget (widget,"drawingarea1");
		if (p->direction == FORWARD)
			angle = p->angle;
		else
			angle = p->angle + M_PI;
		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);
		if (p->next)
		{
			l_start = p->location; l_end = p->next->location;
			ConvertToPixel(&l_start); ConvertToPixel(&l_end);
  			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;
	int direction;
	P.x = parent->location.x;
	P.y = parent->location.y;
	Tree t;
	t.location = P;
	if(!this->search_space)
		return NULL;
	temp = this->search_space;
	while(temp!=NULL)
	{
		if (temp->location.x == P.x && temp->location.y == P.y)
			break;
		temp = temp->next;
	}
	if (!temp) 
	{
	    cout<<"\n	--->>>	Node not found in the search Space :)";
		fflush(stdout);
		return NULL;
	}
	q = NULL;
	for (unsigned int i=0;i<temp->children.size();i++) // Check Each neighbour
	{
		angle = atan2(temp->children[i]->location.y - P.y,temp->children[i]->location.x - P.x); // Angle in Radians
		angle_difference = anglediff(angle,parent->angle);	
		if (angle_difference > DTOR(90)) // Change direction of Motion
		{
			//cout<<"\n Angle difference ="<<RTOD(angle_difference)<<" parent angle="<<RTOD(parent->angle)<<" destination angle="<<RTOD(angle);
			direction = parent->direction * -1;
			//angle += M_PI;
			//if (angle > 2*M_PI ) angle-=2*M_PI;
		}
		else
		{
			direction = parent->direction;
		}
		collides =FALSE;
		parent_angle = parent->angle; if(parent_angle < 0 ) parent_angle += 2*M_PI;
		(direction == FORWARD)?(child_angle = angle):(child_angle = angle + M_PI);
		if(child_angle  < 0 ) child_angle  += 2*M_PI;
		discrete_angle =  parent_angle;
		double start_angle,end_angle;
		start_angle  = (parent_angle > child_angle)?parent_angle:child_angle;
		end_angle	 = (parent_angle < child_angle)?parent_angle:child_angle;
    	discrete_angle =  start_angle;
		//cout<<"\n Start is"<<RTOD(start_angle)<<" End angle="<<RTOD(end_angle);
		for (int s=0 ; s <= ceil(angle_difference/angle_resolution); s++)
		{
			if(Abs(start_angle - end_angle) > DTOR(180))
			{
				discrete_angle += angle_resolution;
				if ( ((discrete_angle>360)?discrete_angle-=360:discrete_angle) > end_angle)
					discrete_angle = end_angle;							
			}
			else
			{
				discrete_angle -= angle_resolution;
				if (discrete_angle < end_angle)
					discrete_angle = end_angle;
			}