astar_sim.h

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

	P.x = parent->NodeInfo.x;
	P.y = parent->NodeInfo.y;
	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
		if ((parent_angle = parent->angle) < 0) parent_angle +=DTOR(360);
		if ((child_angle = angle) < 0) 	        child_angle  +=DTOR(360);
		angle_difference = Absolute(parent_angle - child_angle);
		if (angle_difference > DTOR(120))
			continue;
		discrete_angle =  parent_angle;
		//cout<<"\n I="<<i<<" Diff Angle="<<RTOD(angle_difference);
		//fflush(stdout);
		collides =FALSE;
/*		for (int s=0 ; s <= ceil(angle_difference/angle_resolution); s++)
			{
				if (parent_angle > child_angle)
					{
						discrete_angle -= angle_resolution;
						if (discrete_angle < child_angle)
							discrete_angle = child_angle;
					}
				else
					{
						discrete_angle += angle_resolution;
						if (discrete_angle > child_angle)
							discrete_angle = child_angle;
					}
				if (Obstacle(temp->children[i]->location.x,temp->children[i]->location.y,discrete_angle))
					{
					collides= TRUE;
					break;
					}
			}
*/
		if (!collides)
					{
						p = new Node;
						p->NodeInfo.x = temp->children[i]->location.x;
						p->NodeInfo.y = temp->children[i]->location.y;
						p->nearest_obstacle = temp->obstacle_cost;
						p->parent = parent;
						p->angle= angle;
						p->wheelchair.SetCheckPoints(this->number_of_point_to_check,this->points_to_check);
						p->next = q;
						q = p;
					}
		//cout<<"\n I="<<i<<" Dis Angle="<<RTOD(discrete_angle);
		//fflush(stdout);
		}
	return q;
}
// Free node function
void AstarPlanner :: FreeNode(Node *n) 
{
	delete n;
}

int AstarPlanner :: NodeEquality(Node *a, Node *b) // Test for node equality
{
	if (a->NodeInfo.x == b->NodeInfo.x && a->NodeInfo.y == b->NodeInfo.y)
		return 1;
	return 0;
}
double Magnitude( Point p1, Point p2 )
{
    Point Vector;
    Vector.x = p2.x - p1.x;
    Vector.y = p2.y - p1.y;
    return sqrt( Vector.x * Vector.x + Vector.y * Vector.y);
}
double AstarPlanner :: DistanceToLine( Point LineStart, Point LineEnd, Point P)
{
	double LineMag,distance;
	LineMag = Magnitude(LineStart,LineEnd);
	distance = (P.x*(LineStart.y - LineEnd.y) + P.y*(LineEnd.x - LineStart.x)+(LineStart.x*LineEnd.y - LineEnd.x*LineStart.y))/LineMag ;
    	return distance;
};
double AstarPlanner :: DistToLineSegment(Point LineStart, Point LineEnd, Point p)
{
	Vector2D A(LineStart.x,LineStart.y),B(LineEnd.x,LineEnd.y),P(p.x,p.y);
  	//if the angle between PA and AB is obtuse then the closest vertex must be A
  	double dotA = (P.x - A.x)*(B.x - A.x) + (P.y - A.y)*(B.y - A.y);
  	if (dotA <= 0) 
		return Vec2DDistance(A, P);
	//if the angle between PB and AB is obtuse then the closest vertex must be B
  	double dotB = (P.x - B.x)*(A.x - B.x) + (P.y - B.y)*(A.y - B.y);
   	if (dotB <= 0) 
		return Vec2DDistance(B, P);
   	//calculate the point along AB that is the closest to P
  	//Vector2D Point = A + ((B - A) * dotA)/(dotA + dotB);
	//calculate the distance P-Point
  	//return Vec2DDistance(P,Point);
	return DistanceToLine(LineStart,LineEnd, p); 
}
void  AstarPlanner :: PathFollow(Node * p,double kd, double kt,double ko,double tracking_distance)
{
	/**********************Variable Decleration************************/
  	/* init threads */    
	int direction;
	Point begin,amcl_location,tracking_point,ni,SegmentStart,SegmentEnd,EstimatedPos;
	double angle,prev_angle,theta,error_orientation,displacement,wdem,distance,distance_to_next,obstacle_avoidance_force;
	bool position_found=FALSE,end_reached=FALSE,segment_navigated=FALSE;
	Node  *last,*first;
	double pose[3], pose_var[3][3],minR,minL,minAhead,avoidance_distance_left,avoidance_distance_right,avoidance_distance_ahead;
	Point old_amcl;
    file=fopen("timing.txt","wb");
	PlayerClient robot("127.0.0.1", 6665);
	PositionProxy pp(&robot,0,'a');
	LaserProxy laser(&robot,0,'r');
	LocalizeProxy localizer(&robot,0,'r');
	//robot.SetFrequency(100);
	avoidance_distance_left  = 0.2 + 0.5;
	avoidance_distance_right = 0.3;
	avoidance_distance_ahead = 0.2;
  	SDL_Event event ;
  	if (SDL_Init(SDL_INIT_VIDEO) != 0)
  		{
  		  printf("Unable to initialize SDL: %s\n", SDL_GetError());
  		  return ;
  		}
	//When this program exits, SDL_Quit must be called
  	atexit(SDL_Quit);
  	//Set the video mode to anything, just need a window
  	screen = SDL_SetVideoMode(320, 240, 0, SDL_ANYFORMAT);
  	if (screen == NULL)
   		{
    		printf("Unable to set video mode: %s\n", SDL_GetError());
    		return ;
  		}
	/*****************************************************************/
 	if(localizer.access == 'e')
    		{
  		this->AddText( "\n	--->>> Can't read from localizer <<<---" );
  		exit(-1);
   		} 
 	if(laser.access != 'r')
    		{
      		this->AddText( "	--->>> Can't read from laser" );
      		exit(-1);
    		}  
  	if(pp.GetAccess() == 'e') 
	  	{
    		this->AddText("\n	--->>> Error getting position device access! <<<---");
    		exit(1);
  		}
//	if(WCp.GetAccess() == 'e')
//		{
//		this->AddText("\n	--->>> Error getting wheelchair device atracking_distanceccess! <<<---");
//			exit(1);
//		}		
	printf("%s\n",robot.conn.banner);
	/************** Wheelchair ON Automatic Mode **********************/
//	WCp.SetPower(OFF);
//	usleep(200000);
//	WCp.SetPower(ON);
//	usleep(200000);
//	WCp.SetMode(AUTO);
//	usleep(200000);
//	this->AddText("\n	--->>> WheelChair is ON and CONTROL Mode is AUTO <<<---\n");
//	fflush(stdout);
	/*****************************************************************/
	if (!p) // path not passed throught the arguments 
		return;
	this->ConvertPixel(&this->start);
	//pose[0]= begin.x;
	//pose[1]= begin.y;
	//pose[2]=DTOR(this->initial_angle);
	pose[0]= p->NodeInfo.x;
	pose[1]= p->NodeInfo.y;
	pose[2]=p->angle;
	cout << "\n Default Pose given to the Localizer X="<<p->NodeInfo.x<<" Y="<<p->NodeInfo.y<<" Theta="<<p->angle;
	cout << "\n Tracking Distance="<<tracking_distance<<" Kd="<<kd<<" KTheta="<<kt;
	pose_var[0][0]=0.5;
	pose_var[0][1]=0.5;
	pose_var[0][2]=0.5;
	pose_var[1][0]=0.5;
	pose_var[1][1]=0.5;
	pose_var[1][2]=0.5;
	pose_var[2][0]=0.5;
	pose_var[2][1]=0.5;
	pose_var[2][2]=DTOR(10);
	localizer.SetPose(pose,pose_var);
	/***********************Finding Out where we are from the Localizer with initial estimation ************************/
	while(!position_found) //wait until we have 99% accurate assumption / hypothesis
	{
		if(robot.Read()) 
			exit(1);
		printf("%d hypotheses\n", localizer.hypoth_count); // Assumed number of Hypothesis
    	printf("%d (weight %f): [ %f %f %f ]\n",0,localizer.hypoths[0].weight,localizer.hypoths[0].mean[0],localizer.hypoths[0].mean[1], 	localizer.hypoths[0].mean[2]);
		if(localizer.hypoths[0].weight>=0.9) // Since the hypothesis are sorted, we assume the first one to be the one with the most weight
			{
			position_found=1; // Accurate Hypothesis found, so update current location and move next
			old_amcl.x = EstimatedPos.x=amcl_location.x= localizer.hypoths[0].mean[0];
			old_amcl.y = EstimatedPos.y=amcl_location.y= localizer.hypoths[0].mean[1];
			estimate_theta=localizer.hypoths[0].mean[2];
			localizer.SetPose(pose,pose_var);
			}
	usleep(100000);
	}
	usleep(10000000);
	/********************** Start from the root and move forward ************************/
	first = p;
	prev_angle=p->angle;
	p = p->next; // The first angle is always zero
	/************************************************************************************/
	timer2 = g_timer_new();
	delta_timer = g_timer_new();
	last_time=0;
	delta_t=0;
	velocity=0;
while(!end_reached) 
	{
	g_timer_start(timer2);
	g_timer_start(delta_timer);
	if(robot.Read()) 
		exit(1);
	last = p;
	ni = first->NodeInfo;
	SegmentStart.x = ni.x;
	SegmentStart.y = ni.y;
	printf("\n	--->>>NEW SEGMENT Initial Location of  straight line Path is x[%.3f]y[%.3f] <<<---",SegmentStart.x,SegmentStart.y);
	fflush(stdout);
	ni = last->NodeInfo;
	SegmentEnd.x = ni.x;
	SegmentEnd.y = ni.y;	
	printf("\n	--->>>NEW SEGMENT Last  Location of this straight line Path is x[%.3f]y[%.3f] <<<---",SegmentEnd.x,SegmentEnd.y);
	fflush(stdout);
	direction = -1;
	angle = atan2(SegmentEnd.y - SegmentStart.y,SegmentEnd.x - SegmentStart.x);
	printf("\n	--->>> Angle is:=%.3f <<<---",RTOD(angle));
	//getchar();
	segment_navigated = FALSE;
	while (!segment_navigated) // Loop for each path segment
	{
	while (gtk_events_pending())
    	gtk_main_iteration();
	SDL_PollEvent(&event);
		if(event.type == SDL_KEYDOWN && event.key.keysym.sym == SDLK_q)
			{
				segment_navigated = TRUE;
				end_reached = TRUE;
				cout<<"\n\n Navigation Ended by USER \n\n";
				break;
			}
			//if(robot.Peek(0))
			if(robot.Read()) 
					exit(1);
			if (velocity!=pp.Speed())
				velocity = pp.Speed();
		obstacle_avoidance_force = 0;
		delta_t=g_timer_elapsed(delta_timer, NULL );
		estimate_theta += pp.SideSpeed()*delta_t;
		EstimatedPos.x += velocity*cos(estimate_theta)*delta_t;
		EstimatedPos.y += velocity*sin(estimate_theta)*delta_t;
		printf("\n--->>> Velocity =%.3f m/sev EstimateX=[%.3f] EstimateY=[%.3f] Estimated Theta=[%.3f] time=%g",pp.Speed(),EstimatedPos.x,EstimatedPos.y,RTOD(estimate_theta),delta_t);
		//if (g_timer_elapsed(timer2, NULL ) >=10.0)
		for(int i=0;i<localizer.hypoth_count;i++)
			{
			if(localizer.hypoths[i].weight>=0.8)
				{
				amcl_location.x= localizer.hypoths[i].mean[0];
				amcl_location.y= localizer.hypoths[i].mean[1];
				theta = localizer.hypoths[i].mean[2];
				if(old_amcl.x !=amcl_location.x || old_amcl.y !=amcl_location.y )
					{
						last_time = g_timer_elapsed(timer2, NULL );// Recording the last time Data changed
						g_timer_start(timer2); // resetting the timer
						old_amcl.x=EstimatedPos.x = amcl_location.x;
						old_amcl.y=EstimatedPos.y = amcl_location.y;
						estimate_theta= theta;
					}
				}
			}
		tracking_point.x= EstimatedPos.x + tracking_distance*cos(estimate_theta) - 0*sin(estimate_theta);
		tracking_point.y= EstimatedPos.y + tracking_distance*sin(estimate_theta) + 0*cos(estimate_theta); 
		distance=sqrt(pow(SegmentEnd.x-tracking_point.x,2)+pow(SegmentEnd.y-tracking_point.y,2));
		displacement = DistToLineSegment(SegmentStart,SegmentEnd,tracking_point);
		if (p->next)
			{
				Point n;
				n.x = p->next->NodeInfo.x;
				n.y = p->next->NodeInfo.y;
				distance_to_next = DistToLineSegment(SegmentEnd,n,tracking_point);
			}
		else // This is the last segment
			{
				distance_to_next = 100;
				if (distance <=0.1)
					segment_navigated = TRUE;
			}
		cout <<"\n	--->>>Current disp=["<<displacement<<"] Next =["<<distance_to_next<<"] ";
		if (displacement > distance_to_next) // we are closer to the next segment
			segment_navigated = TRUE;
		if (estimate_theta < 0 )	
			if (angle <0)
				error_orientation = (DTOR(360) + angle) - (estimate_theta + DTOR(360)); // in Radians
			else 
				error_orientation = angle - (estimate_theta + DTOR(360)); // in Radians
		else
			if (angle <0)
				error_orientation = (DTOR(360) + angle) - estimate_theta; // in Radians
			else
				error_orientation = angle - estimate_theta; // in Radians
		if (Absolute(error_orientation) > DTOR(180) ) error_orientation*=-1;
    	minR = minL = minAhead = 1000;
		// Obstacle Avoidance STUFF
/*		for (int j=0; j<laser.scan_count/5; j++) // Right Section
			{
      				if (laser[j] < minR)       		
						minR = laser[j];	
    			}
		for (int j=2*(laser.scan_count/5); j<3*(laser.scan_count/5); j++) // Ahead Section
			{
      				if (laser[j] < minAhead)       		
						minAhead = laser[j];	
    			}
    		for (int j=4*(laser.scan_count/5); j < laser.scan_count; j++) // Left Section
			{
	        		if (laser[j] < minL)
        					minL = laser[j];
    			}
		if (minR < avoidance_distance_right )
			{
			minR = (avoidance_distance_right - minR)/avoidance_distance_right;// inversely propotional to the distance and normalized
			obstacle_avoidance_force += minR;
			}
		if (minAhead < avoidance_distance_ahead)
			{
			minAhead = (avoidance_distance_ahead - minAhead)/avoidance_distance_ahead;// inversely propotional to the distance and normalized
			obstacle_avoidance_force += minAhead;
			//end_reached = TRUE;
			//WCp.SetSpeed(0.0,0.0); 
			pp.SetSpeed(0.0,0.0);
			continue;
			}
		if (minL < avoidance_distance_left )
			{
			minL = (avoidance_distance_left - minL)/avoidance_distance_left;// inversely propotional to the distance and normalized
			obstacle_avoidance_force -= minL;
			}
		cout<<"\n		--->>> Min Distance Left="<<minL<<" Min Distance Right="<<minR;
		fflush(stdout);*/
		wdem = direction*kd*displacement + kt*error_orientation + ko*obstacle_avoidance_force;
		//wdem = ko*obstacle_avoidance_force;
		printf("\n--->>> First X[%.3f]Y[%.3f] Last=X[%.3f]Y[%.3f] Target Angle =[%.3f] Displ=[%.3f] Distance to end=[%.3f] Orient Error=[%.3f] Wdem=[%.3f]", SegmentStart.x,SegmentStart.y ,SegmentEnd.x,SegmentEnd.y ,RTOD(angle),displacement ,distance, RTOD(error_orientation), wdem);
		fprintf(file,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %g %g\n",EstimatedPos.x,EstimatedPos.y,amcl_location.x, amcl_location.y, displacement ,error_orientation ,wdem,SegmentStart.x,SegmentStart.y,SegmentEnd.x,SegmentEnd.y,g_timer_elapsed(delta_timer, NULL ),last_time);
		//pp.SetSpeed(0.1,wdem); 
		if( Absolute(RTOD(error_orientation)) > 60)
			pp.SetSpeed(0.0,wdem);
		else
			pp.SetSpeed(0.1,wdem); 
		g_timer_start(delta_timer);
	}
	this->AddText("\n\n	--->>>Finished Navigating this section, Moving to NEXT --->>>\n");
	first = last;
	if (!p->next)
		{
		printf("\n--->>> Destination Reached !!!");
		end_reached=TRUE;
		fflush(stdout);
		}
	else
		p = p->next;
	}
	pp.SetSpeed(0,0); // Stop The motors
   	g_timer_destroy( timer2 );
   	g_timer_destroy( delta_timer );
	fclose(file);
}