callbacks.c

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

	MapFilename = gtk_file_chooser_get_filename(filechooser);
	if (pixbuf)
		gdk_pixbuf_unref(pixbuf);
	pixbuf = gdk_pixbuf_new_from_file(MapFilename,NULL);
	h = gdk_pixbuf_get_height(pixbuf);
	w = gdk_pixbuf_get_width(pixbuf);
	printf("\nFile name:%s",MapFilename);
}

gboolean
on_drawingarea1_button_release_event   (GtkWidget       *widget,
                                        GdkEventButton  *event,
                                        gpointer         user_data)
{
 switch (event->button)
	{
	case 1: 
		if(pixbuf != NULL)
			SetTargets (widget, event->x, event->y);
		break;
	case 2:		
		printf("\nClearing List");
		while (way_points!=NULL)
			{
			way_temp = way_points;
			way_points = way_points->next;
			delete way_temp;
			}
		break;
	case 3:		
		ConstructWaypoints(widget,event->x,event->y);
		break;
	default : printf("\n Unknown Key Pressed <<<--- Should Never happen ;)");
	};
  return TRUE;
}


gboolean
on_drawingarea1_configure_event        (GtkWidget       *widget,
                                        GdkEventConfigure *event,
                                        gpointer         user_data)
{
	GtkWidget *w;
  	drawable_x = event->x;
  	drawable_y = event->y;
/***************************************************************************** Initialization ****************************************************/
 	if (!initialized)
	{
		MapFilename = "maps/casarea.jpeg"; pixel_size = 0.047;
		//MapFilename = "maps/casareaicp.jpeg"; pixel_size = 0.05;
		//MapFilename = "maps/cas.png"; pixel_size = 0.064;		
  		cout<<"\n Defaults Initialized";
		node_connection_distance = 0.5; // radius of circle inside which the nodes should be connected
		reg_grid_distance = 0.2;	// Distance between nodes generated as a regular grid space
		obstacle_radius = 0.2;		// obstacle expansion Radius
		bridge_length = 1.5;		// length of the bridge segment to be tested
		bridge_gap = 0.1;
		initialized = TRUE;		
		k_distance = 1.5;			// Gain applied on Distance Displacement for path following controller
		k_theta = 2.8;				// Gain applied on Orientation Error for path following controller
		k_obstacle = 0.8;
		tracking_distance = 0.0;	// Distance from the center of motion to the tracking point
		initial_angle = 180;		// Initial Angle Pose of the Robot
		way_points = NULL;
	   	platform = STAGE; 			// Default Platform
	   	
   		w = lookup_widget (widget,"pixel_size");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", pixel_size));
   		
   		w = lookup_widget (widget,"nodes_connection_radius");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.1f", node_connection_distance));

   		w = lookup_widget (widget,"reg_grid_dist");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.1f", reg_grid_distance));

   		w = lookup_widget (widget,"obstacle_radius");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", obstacle_radius));

   		w = lookup_widget (widget,"bridge_length");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", bridge_length));

//   		w = lookup_widget (widget,"pixel_size");
//   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%f", bridge_gap));

   		w = lookup_widget (widget,"k_distance");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_distance));

   		w = lookup_widget (widget,"k_theta");
   		gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_theta));

	}
/***************************************************************************** END Initialization *************************************************/
	printf("\n--->> Configure Event X=%d and Y =%d Height =%d Width=%d",event->x,event->y,event->height,event->width);
	fflush(stdout);
	RefreshDrawingArea(widget); // if the pixbuf == NULL then the refresh function will load the default file automatically
  return TRUE;
}


gboolean
on_drawingarea1_expose_event           (GtkWidget       *widget,
                                        GdkEventExpose  *event,
                                        gpointer         user_data)
{  
  if (pixbuf)
	{
  	printf("\n--->> Expose Event X=%d and Y =%d Height =%d Width=%d",event->area.x,event->area.y,event->area.height,event->area.width);
	fflush(stdout);
 	gtk_widget_set_size_request(widget,w,h);
  	gdk_draw_pixbuf(widget->window, NULL, pixbuf, 
                    event->area.x, event->area.y,
                    event->area.x, event->area.y,
                    event->area.width, event->area.height,
                    GDK_RGB_DITHER_NORMAL, 0, 0);
	if(Planner)
		{
//			Planner->draw_tree();	
			if(Planner->path)
				Planner->draw_path();
		}
	}
  return FALSE;
}


gboolean
on_drawingarea1_motion_notify_event    (GtkWidget       *widget,
                                        GdkEventMotion  *event,
                                        gpointer         user_data)
{
  int x, y;
  GdkModifierType state;
  if (event->is_hint)
    gdk_window_get_pointer (event->window, &x, &y, &state);
  else
    {
     mouse_location.x = x = (int) event->x;
     mouse_location.y = y = (int) event->y;
      //state = event->state;
    }
  cout<<"\n Mouse Location is x="<<mouse_location.x<<" y="<<mouse_location.y;
  fflush(stdout);
  if (state & GDK_BUTTON1_MASK && pixbuf != NULL)
	{
    		SetTargets (widget, x, y);
		//gdk_draw_point(widget,NULL,x,y);
	}
  return TRUE;
}


void
on_new1_activate                       (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_open1_activate                      (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{
 //GtkWidget *chooser;
 //chooser = create_filechooserdialog1();
 //gtk_widget_show_all(chooser);

}


void
on_save1_activate                      (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_save_as1_activate                   (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_quit1_activate                      (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_cut1_activate                       (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_copy1_activate                      (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_paste1_activate                     (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_delete1_activate                    (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}


void
on_about1_activate                     (GtkMenuItem     *menuitem,
                                        gpointer         user_data)
{

}

void
on_tracking_distance_value_changed     (GtkRange        *range,
                                        gpointer         user_data)
{
	tracking_distance = gtk_range_get_value(range);
	//if(tracking_distance)
		//add_text (GTK_WIDGET(range),g_strdup_printf("\n->Tracking Distance changed to:%.3f", tracking_distance));
}


void
on_Linea_speed_value_changed           (GtkRange        *range,
                                        gpointer         user_data)
{
	linear_speed = gtk_range_get_value(range);
	//if(linear_speed)
		//add_text (GTK_WIDGET(range),g_strdup_printf("\n->Linear Speed changed to:%.3f", linear_speed));
	if(Follower)
		Follower->speed = linear_speed;
}		


void
on_safety_distance_value_changed       (GtkRange        *range,
                                        gpointer         user_data)
{
	safety_distance = gtk_range_get_value(range);
//	if(safety_distance)
		//add_text (GTK_WIDGET(range),g_strdup_printf("\n->Safety Distance changed to:%.3f", safety_distance));
}


void
on_startx_value_changed                (GtkRange        *range,
                                        gpointer         user_data)
{
GtkWidget * w;
	start.x = gtk_range_get_value(range);
        w = lookup_widget (GTK_WIDGET(range),"entry1");
        gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", start.x));
}


void
on_starty_value_changed                (GtkRange        *range,
                                        gpointer         user_data)
{
GtkWidget * w;
        w = lookup_widget (GTK_WIDGET(range),"entry2");
        gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", gtk_range_get_value(range)));
	start.y = gtk_range_get_value(range);
}


void
on_start_theta_value_changed           (GtkRange        *range,
                                        gpointer         user_data)
{
GtkWidget * w;
        w = lookup_widget (GTK_WIDGET(range),"entry3");
        gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", gtk_range_get_value(range)));
}


void
on_targetx_value_changed               (GtkRange        *range,
                                        gpointer         user_data)
{
GtkWidget * w;
        w = lookup_widget (GTK_WIDGET(range),"entry4");
        gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", gtk_range_get_value(range)));
	end.x = gtk_range_get_value(range);
}


void
on_targety_value_changed               (GtkRange        *range,
                                        gpointer         user_data)
{
GtkWidget * w;
        w = lookup_widget (GTK_WIDGET(range),"entry5");
        gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.3f", gtk_range_get_value(range)));
	end.y = gtk_range_get_value(range);
}


void