astar.h

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

/***************************************************************************
 *   Copyright (C) 2005 by Tarek Taha                                      *
 *   [e-mail]                                                 *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#define OPEN 1
#define NEW 0
#define CLOSED 2
#include <unistd.h> 
#include <time.h> 
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <string.h>
#include <error.h>
#include "SDL/SDL.h"
#include <list>
#include <gdk-pixbuf/gdk-pixbuf.h>
#include <gtk/gtkmain.h>
#include<gtk/gtk.h>
#include<glib/gprintf.h>
#include <assert.h>
#include <playerclient.h>
#include "common.h"
#include "wheelchairproxy.h"
#define max_speed 0.1
#define max_turn_rate 0.2 // wheelchair negative rotations
#define FORWARD 1
#define BACKWARD -1
#include "map.h"
#include "callbacks.h"
#include "interface.h"
#include "support.h"
enum {WHEELCHAIR,STAGE};
GTimer *timer2,*delta_timer;
double last_time;
double estimate_x,estimate_y,estimate_theta,velocity,delta_t;
/*
 * Max
 * Return the maximum of two numbers.
 */
#define Max(x, y) ((x) > (y) ? (x) : (y))
/*
 * Min
 * Return the minimum of two numbers.
 */
#define Min(x, y) ((x) < (y) ? (x) : (y))
/*
 * Abs
 * Return the absolute value of the argument.
 */
#define Abs(x) ((x) >= 0 ? (x) : -(x))
/* This function takes two angles in radians
 * and returns the smallest angle between them in radians
 */
double anglediff(double alfa, double beta) 
{
	double diff;
	if( alfa < 0 ) alfa+= 2*M_PI; 	if( alfa > 2*M_PI) alfa-= 2*M_PI;
	if( beta < 0 ) beta+= 2*M_PI;	if( beta > 2*M_PI) beta-= 2*M_PI;		
	diff = alfa - beta;
	if ( diff >  M_PI) diff=( 2*M_PI  - diff);
	if ( diff < -M_PI) diff=(-2*M_PI - diff);
	return Abs(diff);
};
class Point
	{
	public : 
		double x,y;
		Point(double x, double y);
		Point();
	};
Point::Point()
	{
		this->x = 0;
		this->y = 0;
	};
Point::Point(double x,double y)
	{
	this->x = x;
	this->y = y;
	};
class Robot
	{
	public :
		//Point * check_points;
		vector<Point> check_points;
		void SetCheckPoints(int,Point *);
		Robot();
		~Robot();	
	};
void Robot::SetCheckPoints(int number_of_points,Point * a) 
	{
	//this->check_points = new Point[number_of_points];
	for(int i = 0; i < number_of_points; i++)
		{
		check_points.push_back(a[i]);
		//cout << "\n New Robot i="<<i<<" X=" <<this->check_points[i].x<<" Y="<<this->check_points[i].y;
		}
	};
Robot::Robot() 
	{
	};
Robot::~Robot() 
	{
	//delete [] this->check_points;
	//cout << "\n Robot Freed ";
	};
class Node
	{
	public :
		int id, depth,direction;
		double nearest_obstacle,g_value,h_value,f_value,angle;
		Node  * parent, * next, * prev;
		Point   location;
		Robot  wheelchair; // saving the location of the Robot edges on the planned path
		Node ();
		~Node();	
	};
Node :: Node ()
	{
	parent = next = prev = NULL;
	};
Node :: ~Node ()
	{
	//cout << "\n Node Freed ";
	parent = next = prev = NULL;
	};
class SearchSpaceNode
	{
	public :
		Point location;
		SearchSpaceNode * parent, * next;
		double obstacle_cost;
		vector <SearchSpaceNode *>  children;
		 SearchSpaceNode ();
		~SearchSpaceNode ();
	};
SearchSpaceNode::SearchSpaceNode()
	{
	parent = next = NULL;
	};
SearchSpaceNode::~SearchSpaceNode()
	{
	parent = next = NULL;
	};
int NodeEquality(Node *a, Node *b) // Test for node equality
{
	if (a->location.x == b->location.x && a->location.y == b->location.y)
		return 1;
	return 0;
}
class LList
	{
		public :
			Node * 	Start;
		public :
			     	LList();
			     	~LList();
			void 	Add(Node *);
			bool 	Remove(Node *);
			void 	Print();
			void	Free();
			Node   *Find(Node *);
			void   	Next();
			void    Prev();
			Node *  GetHead();
	};

Node * LList::GetHead()
	{
		return this->Start;
	};
void LList::Next()
	{
		Start = Start->next;
		if(Start != NULL)
			Start->prev = NULL;
	};
void LList::Prev()
	{
		Start = Start->prev;
	};
Node * LList::Find(Node * q)
	{
		Node *p = Start;
		while (p)
		{
			if (NodeEquality(q,p))
				return p;
			p = p->next;				
		}
		return NULL;
	};
LList::LList()
	{
		Start = NULL;
	};
LList::~LList()
	{
		Node *p;
		while (Start != NULL)
		{
			p = Start->next;
			delete Start;
			Start = p;
  		}		
	};
void LList::Free()
	{
		Node *p;
		while (Start != NULL)
		{
			//cout <<"\n	--->>> Freeing Node <<<---"; fflush(stdout);
			p = Start->next;
			delete Start;
			Start = p;
  		}		
	};
void LList::Add(Node * curChild)
	{
		Node * p,* q ;
		p = this->Start;
		q = p;
		// now insert the child into the open list according to the f value
		while (p) 
		{
			// insert before p, sorted ascending
			if (p->f_value >= curChild->f_value) 	       
			{
				// test head of the list case
				if (p == Start)
					Start = 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)       
		{
			if (q != NULL) // insert at the end
			{
	      		q->next = curChild;
  				curChild->prev = q;
  				curChild->next = NULL;
			}
			else	      // insert at the beginning
			{
				Start = curChild;
				curChild->prev = NULL;
			}
		}			
		//cout <<"\n	--->>> Node Added Successfully <<<---"; fflush(stdout);
	};
bool LList::Remove(Node *q)
	{
		Node *p;
		p = Start;
		while (p)
		{
			if(NodeEquality(p,q))
				{
					if (p->prev != NULL)	        
			      		(p->prev)->next = p->next;
					if (p->next != NULL)	        
						(p->next)->prev = p->prev;								
					if (p == Start)
						Start = p->next;
					delete p;
					return 1;
				}
			p = p->next;
		}
		return 0;	
	};
void LList::Print()
	{
		Node *p;
		int i=0;
		p = Start;
		while(p)
		{
			cout<<"\n Node["<<++i<<"] X="<<p->location.x<<" Y="<<p->location.y;
			p = p->next;
		}
	};
typedef struct _tree
	{
		Point location;
		vector <Point> children;
	}Tree;
typedef struct _control_action
	{
		double linear_velocity;
		double angular_velocity;
	} ControlAction;
class AstarPlanner 
	{
	public :
		Robot * wheelchair; // Location of the wheelchair's points to check in the wheelchair coordinate system
		int number_of_point_to_check,map_height,map_width,MAXNODES;
		double pixel_size,initial_angle,obstacle_radius,node_gap,bridge_length,final_angle;
		bool simulate;
		Point start, end, * points_to_check,local_edge_points[4],translated_edge_points[4];
		const char * MapFilename;
		GtkWidget * widget;
		GdkPixbuf * pixbuf;
		MapInfo mapinfo;	
		Node * test,*root,*path, *p;
		SearchSpaceNode * search_space,* temp;
		Node *current, *childList, *curChild, *q;
		LList *openList,*closedList;
		//list <Node> openList,closedList;
		MapFile * Map;
		//PathFollower follower;
		vector <Tree> tree;
	public :
		double Calculate_g   (Node *); // Distance from Start
		double Calculate_h   (Node *); // Herustic Distance to Goal
		bool   GoalReached   (Node *); 
		Node  *MakeChildrenNodes(Node *parent) ;
		void   FreeNode      (Node *);
		void   PrintNodeList ();
		int    StartSearch   (Point start, Point end,double initial_angle,double final_angle);
		void   Translate(Point  , double);
		void   Translate_edges(Point  , double);
		int    check_line (Point start,Point end);
		int    Obstacle   (Point p, double angle);
		void   ConvertPixel   (Point *p);
		void   ConvertToPixel (Point *p);
		void   ReadMap(); // Reads the Map file and sets the attributes
		void   ExpandObstacles();
		void   AddCostToNodes(double distance);
		void   BridgeTest(double length,double gap);
		bool   CheckShortestDistance(double i,double j,double neigbhour_pixel_distance);
		void   GenerateRegularGrid(double gap);
		void   ConnectNodes(double distance);
		void   ShowConnections();
		void   SaveSearchSpace();
		void   DetermineCheckPoints();
		void   AddText ( char const * text);
		void   FindRoot();
		void   draw_path();
		void   draw_tree();
		void   Print();
		void   FreePath();
		void   PathFollow(Node *,double kd, double kt,double ko,double tracking_distance);
		AstarPlanner(Point start,Point end,double initial_angle,double final_angle,double pixel_size,double radius,GtkWidget*,const char * MapFilename); 
		AstarPlanner();
		~AstarPlanner();
	};
AstarPlanner :: AstarPlanner()
	{
	};
void AstarPlanner::FreePath()
	{
	while(path != NULL) 
		{
		p = path->next;
		delete path;
		path = p;
		}
	};
void AstarPlanner::DetermineCheckPoints() // This Determines the locations of the points to be checked in the Vehicle Coordinates, should be rotated at each node
	{
	//Point edges[4]={{32.5,93},{-32.5,93},{-32.5,-22},{32.5,-22}}; this is the exact dimentions
	int point_index=0,points_per_height,points_per_width;
	double i,j;
	double l = 1.2 , w = 0.7;   // length and width of the wheelchair, can be modefied at any time.
	double startx,starty;       // The edges of the robot in -ve quadrant
	Point center_of_rotation;   // center of rotation in respect to the center of Area
	center_of_rotation.x =-0.3; // it's 30 cm on the x-axis
	center_of_rotation.y = 0;   // it's on the mid of the Wheels Axes so i assume that y = 0;
	startx = -l/2 - center_of_rotation.x; // am determining here the location of the edges in the robot coordinate system
	starty = -w/2 - center_of_rotation.y; // 
	local_edge_points[0].x = startx; 		local_edge_points[0].y = starty;
	local_edge_points[1].x = startx;		local_edge_points[1].y = w + starty;
	local_edge_points[2].x = l + startx;	local_edge_points[2].y = w + starty;
	local_edge_points[3].x = l + startx; 	local_edge_points[3].y = starty;
	// These Points are used for drawing the Robot rectangle
	for (int i=0 ;i < 4; i++)
		cout<<"\nEdge->"<< i<<" X="<<local_edge_points[i].x<<" Y="<<local_edge_points[i].y;
	// Create a Matrix of the points to check for collision detection
	points_per_height = (int)(ceil((l) / (2.0*this->obstacle_radius)));
	points_per_width  = (int)(ceil((w) / (2.0*this->obstacle_radius)));
	this->number_of_point_to_check = points_per_height*points_per_width;
	cout<<"\nPer H ="<<points_per_height<<" Per W="<<points_per_width<<" Total ="<<this->number_of_point_to_check;
	cout<<"\n Obstacle Radius="<<this->obstacle_radius; fflush(stdout);

	// The location of the current edges at each NODE
	this->points_to_check = new Point[this->number_of_point_to_check];
	i =(startx + this->obstacle_radius);
	for(int r =0; r < points_per_height ; r++ )
	{
		j=(starty + this->obstacle_radius);
		for (int s=0;s < points_per_width;s++)
		{
			// Angle zero is when robot heading points to the right (right had rule)
			this->points_to_check[point_index].x = i;
			this->points_to_check[point_index].y = j;
			point_index++;
			//cout<<"\n I="<<i<<" J="<<j;
			if ( (j+2*this->obstacle_radius) >= (w + starty) ) 
				j += (w + starty - j)/2;
			else 
				j += (2*this->obstacle_radius);