astar_sim.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 <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 ClockWise -1
#define AntiClockWise 1
#include "map.h"
#include "callbacks.h"
#include "interface.h"
#include "support.h"
SDL_Surface *screen;
FILE * file;
GTimer *timer2,*delta_timer;
double last_time;
double estimate_x,estimate_y,estimate_theta,velocity,delta_t;
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,state;
		double nearest_obstacle,g_value,h_value,f_value,angle;
		Node  * parent, * next, * prev;
		Point   NodeInfo;
		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;
	};
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 *openList, *closedList, *current, *childList, *curChild, *q;
		MapFile * Map;
	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 *);
		int    NodeEquality  (Node *, 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);
		double DistToLineSegment(Point LineStart, Point LineEnd, Point p);
		int    Obstacle   (double x, double y, double angle);
		void   ConvertPixel   (Point *p);
		void   ConvertToPixel (Point *p);
		double Absolute(double );
		double DistanceToLine(Point p1,Point p2, Point p3);
		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   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 , w = 0.6;   // 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;
	//  TO BE DONE in a smarter and GENERAL WAY / not a priority
	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;
	for (int i=0 ;i < 4; i++)
		cout<<"\nEdge->"<< i<<" X="<<local_edge_points[i].x<<" Y="<<local_edge_points[i].y;
	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);
	//cout<<"L+startx = "<<l+startx<<"W+starty="<<w+starty;
	for(int r =0; r < points_per_height ; r++ )
		{
			j=(starty + this->obstacle_radius);
			for (int s=0;s < points_per_width;s++)
			{
				this->points_to_check[point_index].x = i;// Angle zero is when robot heading points to the right (right had rule)
				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);
			}
			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
	};
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 && distance >= 0.2)
				{
					angle = atan2(S->location.y - temp->location.y ,S->location.x - temp->location.x);
					if(!Obstacle(S->location.x,S->location.y,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;