callbacks.c

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

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
#include <gtk/gtk.h>
#include <stdlib.h>
Point start, end,mouse_location;
const char * MapFilename;
int platform;
AstarPlanner * Planner=NULL;
PathFollower * Follower=NULL;
static GdkPixbuf *pixbuf=NULL;
bool start_activated=FALSE,end_activated=FALSE,initialized=FALSE,initialize_angle=FALSE;
gint drawable_x,drawable_y,h,w;
double initial_angle,final_angle,pixel_size,tracking_distance,linear_speed,safety_distance,obstacle_radius,
       pixels_per_tile,node_connection_distance,reg_grid_distance,bridge_length,k_distance,k_theta,bridge_gap,k_obstacle;
GtkWidget *temp, * view;
GtkTextBuffer *text_buffer;
GtkTextIter     startv, endv;
GTimer *timer;
GdkGC *gc = NULL;
GdkColor blue = {0};
Node * way_points,*way_temp,*current;
typedef struct
	{
	double kd,kt,tracking_distance;
	} thread_args;
thread_args *args = (thread_args *) args;
bool threading= TRUE;
bool st=TRUE;
void ReadPixBufferFromFile(const char * filename) // Reads a pixbuff from an RBG Map
{
	pixbuf = gdk_pixbuf_new_from_file(filename,NULL);
	h = gdk_pixbuf_get_height(pixbuf);
	w = gdk_pixbuf_get_width(pixbuf);
}
void RefreshDrawingArea(GtkWidget * widget) // Refreshes the Drawing Area by redrawing the pixel Buffer.
{
	if (!pixbuf)
			ReadPixBufferFromFile(MapFilename);
	temp = lookup_widget (GTK_WIDGET(widget),"drawingarea1");
  	gdk_draw_pixbuf(temp->window, NULL, pixbuf, 
		    	0, 0,
                    	0, 0,
                       -1,-1,
                    	GDK_RGB_DITHER_NORMAL, 0, 0);
	gtk_widget_set_size_request(temp,w,h);
};
void RedrawMap(GtkWidget * widget) // Redraws the Map from the saved and buffered file, Was used for testing only
	{
  	gchar *spacefile,*file;
	if (pixbuf)
		gdk_pixbuf_unref(pixbuf);
  	file=g_strdup(MapFilename);
  	spacefile=strchr(file,'.');
  	if(spacefile!=NULL)
  		*spacefile='\0';   
  	spacefile=g_strdup_printf("%s%s",file,"_FreeSpace.jpeg");
	pixbuf = gdk_pixbuf_new_from_file(spacefile,NULL);
	h = gdk_pixbuf_get_height(pixbuf);
	w = gdk_pixbuf_get_width(pixbuf);
	RefreshDrawingArea(widget);
	};
static void scroll(GtkWidget *v) // Supposed to scroll to the end of the Text View for some reason, that i will discover later, it's not working
{
	GtkTextBuffer	*buffer;
	GtkTextIter	start,end;
	GtkTextMark* mark;
	if (!v)
		return;
	buffer = gtk_text_view_get_buffer(GTK_TEXT_VIEW(v));
    gtk_text_buffer_get_bounds (buffer, &start, &end);
   	mark = gtk_text_buffer_create_mark    (buffer, NULL, &end, 1);
   	gtk_text_view_scroll_to_mark(GTK_TEXT_VIEW(v), mark, 0.0, 0, 0.0, 1.0);
   	gtk_text_buffer_delete_mark (buffer, mark);
}
void add_text (GtkWidget * widget, char const * text) // Adds text to the Text View Widget
{
	view = lookup_widget (GTK_WIDGET(widget),"textview1");
  	text_buffer = gtk_text_view_get_buffer (GTK_TEXT_VIEW (view));
	gtk_text_buffer_get_end_iter(text_buffer, &endv);
	gtk_text_buffer_insert(text_buffer, &endv, text, -1);
	scroll(view);
}
void draw_pixel (int red,int green,int blue,int i,int j) // Draws a pixel of certain RBG colors into the pix buffer
	{
  	int rowstride=0, n_channels, bps;
  	guchar *pixels;
  	guchar * p;
  	rowstride = gdk_pixbuf_get_rowstride(pixbuf);
  	bps = gdk_pixbuf_get_bits_per_sample(pixbuf)/8;
  	n_channels = gdk_pixbuf_get_n_channels(pixbuf);
  	pixels = gdk_pixbuf_get_pixels(pixbuf);
  	if(gdk_pixbuf_get_has_alpha(pixbuf))
  		n_channels++;
  	p= pixels +j*rowstride + i*n_channels;
  	p[0]=red;
  	p[1]=green;
  	p[2]=blue;
  	//p[3]=;
	  return;
	}
void draw_path(GtkWidget * w)
{
  if (!Planner || !Planner->path)
	{
		add_text (GTK_WIDGET(w),g_strdup_printf("\n->NO Path Generated Yet, plan a Path First"));
		return;
	}

	Point l_start,l_end;
  	gchar *savefile,*file;
  	//int step=0;
  	Node *p;
  	p = Planner->path;
	while(p != NULL && p->next!=NULL)
	{
		l_start =  p->location;
		Planner->ConvertToPixel(&l_start);
		l_end   =  p->next->location;
		Planner->ConvertToPixel(&l_end);
		//add_text (GTK_WIDGET(w),g_strdup_printf("\n--> Step %03d: (%.4f, %.4f)", ++step, l_start.x, l_start.y));
  		temp = lookup_widget (w,"drawingarea1");
		if(p->parent!=NULL)
		{
//		for(int i=0;i<Planner->number_of_point_to_check;i++)
//			{

//			}
 		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)p->wheelchair.check_points[0].x,(int)p->wheelchair.check_points[0].y,
		(int)p->wheelchair.check_points[4].x,(int)p->wheelchair.check_points[4].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)p->wheelchair.check_points[4].x,(int)p->wheelchair.check_points[4].y,
		(int)p->wheelchair.check_points[5].x,(int)p->wheelchair.check_points[5].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)p->wheelchair.check_points[5].x,(int)p->wheelchair.check_points[5].y,
		(int)p->wheelchair.check_points[1].x,(int)p->wheelchair.check_points[1].y);
  		gdk_draw_line (temp->window,(GdkGC*)(temp)->style->white_gc,(int)p->wheelchair.check_points[1].x,(int)p->wheelchair.check_points[1].y,
		(int)p->wheelchair.check_points[0].x,(int)p->wheelchair.check_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;
	} 
  	cout << "\n Path Drawn";
  	file=g_strdup(MapFilename);
  	savefile=strchr(file,'.');
  	if(savefile!=NULL)
  		*savefile='\0';   
  	savefile=g_strdup_printf("%s%s",file,"_PATH.jpeg");
  	gdk_pixbuf_save(pixbuf,savefile,"jpeg",NULL,NULL);//save the file
  	g_free(savefile);
  	g_free(file);
	//RefreshDrawingArea(w);
};
void draw_pixel_path(GtkWidget * w) // This draws the path generated by the 1 pixel/Tile using A start, Just draws Pixels
				    // Then it saves the generated path from the pixbuffer into the file adding _PATH to the file name
{
  if (!Planner->path)
	{
		add_text (GTK_WIDGET(w),g_strdup_printf("\n->NO Path Generated Yet, plan a Path First"));
		return;
	}
  	gchar *savefile,*file;
  	int step=0;
  	Node *p;
  	p = Planner->path;
	Point ni;
	while(p != NULL )
	{
		ni = p->location;
		add_text (GTK_WIDGET(w),g_strdup_printf("\n--> Step %03d: (%d, %d)", ++step, (int)ni.x, (int)ni.y));
		draw_pixel(0x14,0x08,0xff,(int)ni.x,(int)ni.y);
		if(p->parent==NULL)
			cout<<"\nI AM THE ROOOT";
		for(int i=0;i<4;i++)
			{
			draw_pixel(0x2c,0xff,0x0f,(int)(p->wheelchair.check_points[i].x),(int)(p->wheelchair.check_points[i].y));
			}
		fflush(stdout);
  		temp = lookup_widget (w,"drawingarea1");
		p = (Node *)p->next;
	} 
  	cout << "\n Path Drawn";
  	file=g_strdup(MapFilename);
  	savefile=strchr(file,'.');
  	if(savefile!=NULL)
  		*savefile='\0';   
  	savefile=g_strdup_printf("%s%s",file,"_PATH.jpeg");
  	gdk_pixbuf_save(pixbuf,savefile,"jpeg",NULL,NULL);//save the file
  	g_free(savefile);
  	g_free(file);
	RefreshDrawingArea(w);
};
void ConstructWaypoints(GtkWidget * w,double x,double y) // Using the mouse right button we can generate a path , this function does that ;)
	{
	Point s,line_start,line_end;
	if(!Planner)
		{
		Planner = new AstarPlanner(start,end,initial_angle,final_angle,pixel_size,obstacle_radius,w,MapFilename);
		Planner->ReadMap();
		}
	s.x=x;
	s.y=y;
	Planner->ConvertPixel(&s);
	way_temp = new Node;
	way_temp->location.x = s.x;
	way_temp->location.y = s.y;
	way_temp->parent = NULL;
	way_temp->prev = NULL;
	way_temp->angle= 0;
	way_temp->next =NULL;
	if (!way_points)
		{
		way_points = way_temp;
		current = way_temp;
		}
	else
		current->next = way_temp;
	if (current && current->next)
	{
		line_start.x = current->location.x;
		line_start.y = current->location.y;
		line_end.x   = current->next->location.x;
		line_end.y   = current->next->location.y;
		way_temp->parent = current;
		current->angle = atan2(current->next->location.y - current->location.y  ,current->next->location.x - current->location.x);
		Planner->ConvertToPixel(&line_start);
		Planner->ConvertToPixel(&line_end);
		printf("\n Start X[%.3f]Y[%.3f] End X[%.3f]Y[%.3f]",line_start.x,line_start.y,line_end.x,line_end.y);
  		temp = lookup_widget (w,"drawingarea1");
  		gdk_draw_line(temp->window,(GdkGC*)(temp)->style->white_gc,(int)line_start.x,(int)line_start.y,(int)line_end.x,(int)line_end.y);
	}
	current = way_temp;
	};
static void SetTargets (GtkWidget *widget, gdouble x, gdouble y) // Captures the left mouse clicks and uses them to set the source and target locations
{
  GtkWidget *w;
  //GdkGC *gc;
  //GdkColormap *colormap;
  GdkColor      mycolor         = { 0, 0x15, 0x3a, 0xfa };
  //GdkGCValues values;
  Point s,e;
	if(!Planner)
		{
		Planner = new AstarPlanner(start,end,initial_angle,final_angle,pixel_size,obstacle_radius,w,MapFilename);
		Planner->ReadMap();
		}
  mycolor.red   = 0x12;
  mycolor.green = 0xf2;
  mycolor.blue  = 0xfa;
  temp = lookup_widget (widget,"drawingarea1"); // Draws a rectangle aroung the target and destination locations.-90
  if (! start_activated ) // Sets the starting Location
	{
	if (!initialize_angle)
		{
			RefreshDrawingArea(widget);
			start.x = x;
			start.y = y;
			printf("\n---> Setting Source X=%f Y=%f",x,y);
			fflush(stdout);
        		w = lookup_widget (widget,"entry1");
        		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", x));
        		w = lookup_widget (widget,"entry2");
        		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", y));
			initialize_angle = TRUE;
			gdk_draw_arc (temp->window,(GdkGC*)(temp)->style->white_gc,TRUE,(int)x-5,(int)y-5,10,10,0,360*64);
		}
	else
		{	
			s.x = start.x;
			s.y = start.y;
			e.x = x;
			e.y = y;
			Planner->ConvertPixel(&s);
			Planner->ConvertPixel(&e);
			initial_angle = atan2(e.y - s.y,e.x - s.x);
			cout<<"\n---> Initial Angle is= "<<RTOD(initial_angle);
        		w = lookup_widget (widget,"entry3");
        		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", RTOD(initial_angle)));
			start_activated  = TRUE;
			initialize_angle = FALSE;
			gdk_draw_line(temp->window,(GdkGC*)(temp)->style->white_gc,(int)start.x,(int)start.y,(int)x,(int)y);
		}
	}
  else // Sets the target Location
	{
	if (!initialize_angle)
		{
			end.x = x;
			end.y = y;
			printf("\n<--- Setting Target X=%f Y=%f",x,y);
			fflush(stdout);
        		w = lookup_widget (widget,"entry4");
	        	gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", x));
        		w = lookup_widget (widget,"entry5");
        		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", y));
			initialize_angle = TRUE;
			gdk_draw_arc (temp->window,(GdkGC*)(temp)->style->white_gc,TRUE,(int)x-5,(int)y-5,10,10,0,360*64);
		}
	else
		{
			s.x = end.x;
			s.y = end.y;
			e.x = x;
			e.y = y;
			Planner->ConvertPixel(&s);
			Planner->ConvertPixel(&e);
			final_angle = atan2(e.y - s.y,e.x - s.x);
        		w = lookup_widget (widget,"entry6");
        		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", RTOD(final_angle)));
			cout<<"\n<--- Final Angle is= "<<RTOD(final_angle);
			fflush(stdout);
			initialize_angle = FALSE;
			start_activated = end_activated =FALSE;
			gdk_draw_line(temp->window,(GdkGC*)(temp)->style->white_gc,(int)end.x,(int)end.y,(int)x,(int)y);
		}
	}  
}
void
on_filechooserbutton1_selection_changed
                                        (GtkFileChooser  *filechooser,
                                        gpointer         user_data)
{