callbacks.c

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

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


void
on_pixel_size_changed                  (GtkEditable     *editable,
                                        gpointer         user_data)
{
	pixel_size = atof(gtk_editable_get_chars(editable,0,-1));
//	if(pixel_size)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Pixel Size changed to:%.3f", pixel_size));
}


void
on_pixels_per_tile_changed             (GtkEditable     *editable,
                                        gpointer         user_data)
{
	pixels_per_tile = atof(gtk_editable_get_chars(editable,0,-1));
//	if(pixels_per_tile)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Pixels Per Tile changed to:%.3f", pixels_per_tile));
}


void
on_obstacle_radius_changed             (GtkEditable     *editable,
                                        gpointer         user_data)
{
	obstacle_radius = atof(gtk_editable_get_chars(editable,0,-1));
//	if(obstacle_radius)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Obstacle Radius changed to:%.3f", obstacle_radius));
}


void
on_bridge_length_changed               (GtkEditable     *editable,
                                        gpointer         user_data)
{
	bridge_length = atof(gtk_editable_get_chars(editable,0,-1));
//	if(bridge_length)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Bridge Length changed to:%.3f", bridge_length));
}


void
on_k_distance_changed                  (GtkEditable     *editable,
                                        gpointer         user_data)
{
	k_distance = atof(gtk_editable_get_chars(editable,0,-1));
	//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->K-Distance changed to:%.3f", k_distance));
	if(Follower)
		Follower->kd = k_distance;
}


void
on_k_theta_changed                     (GtkEditable     *editable,
                                        gpointer         user_data)
{
	k_theta = atof(gtk_editable_get_chars(editable,0,-1));
	//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->K-theta changed to:%.3f", k_theta));
	if(Follower)
		Follower->kt = k_distance;	
}



void
on_entry1_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	start.x=atof(gtk_editable_get_chars(editable,0,-1));
//	if(start.x)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Start X changed to:%.3f", start.x));
}


void
on_entry2_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	start.y=atof(gtk_editable_get_chars(editable,0,-1));
//	if(start.y)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Start Y changed to:%.3f", start.y));
}


void
on_entry3_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	initial_angle=DTOR(atof(gtk_editable_get_chars(editable,0,-1)));
//	if(start.y)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Initial Angle changed to:%.3f", RTOD(initial_angle)));
}


void
on_entry4_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	end.x=atof(gtk_editable_get_chars(editable,0,-1));
//	if(end.x)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->End X changed to:%.3f", end.x));
}


void
on_entry5_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	end.y=atof(gtk_editable_get_chars(editable,0,-1));
//	if(end.y)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->End Y changed to:%.3f", end.y));
}


void
on_entry6_changed                      (GtkEditable     *editable,
                                        gpointer         user_data)
{
	//theta=atof(gtk_editable_get_chars(editable,0,-1));
}
void
on_generate_cspace_released            (GtkButton       *button,
                                        gpointer         user_data)
{
	if (Planner)
		delete Planner;
	timer = g_timer_new();
	Planner = new AstarPlanner(start,end,initial_angle,final_angle,pixel_size,obstacle_radius,GTK_WIDGET(button),MapFilename);
	Planner->ReadMap();
	while (gtk_events_pending())
		gtk_main_iteration();
	Planner->ExpandObstacles();
	add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Obstacle Expansion took:%gs", g_timer_elapsed( timer, NULL )));
	g_timer_start(timer);
	while (gtk_events_pending())
		gtk_main_iteration();
	Planner->GenerateRegularGrid(reg_grid_distance);
	add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Grid Generation took:%gs", g_timer_elapsed( timer, NULL )));
	g_timer_start(timer);
	while (gtk_events_pending())
		gtk_main_iteration();
	Planner->BridgeTest(bridge_length,bridge_gap); // m gap between nodes
	add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Bridge Test took:%gs", g_timer_elapsed( timer, NULL )));
    g_timer_destroy( timer );
	while (gtk_events_pending())
		gtk_main_iteration();
	Planner->AddCostToNodes(3);
	Planner->ConnectNodes(node_connection_distance); // Connect Nodes that are within certain radius
	while (gtk_events_pending())
		gtk_main_iteration();
	if (pixbuf)
		gdk_pixbuf_unref(pixbuf);
	pixbuf = Planner->pixbuf;
	RefreshDrawingArea(GTK_WIDGET(button));
	Planner->ShowConnections();
}


void
on_path_plan_released                  (GtkButton       *button,
                                        gpointer         user_data)
{
	if (!Planner)
		{
		add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Initialize Planner First ..."));
		return;
		}
	if (!Planner->search_space)
		{
		add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Generate Search Space first ..."));
		return;
		}
	timer = g_timer_new();
	Planner->FreePath(); // Free previously generated Path if exists
	Planner->StartSearch(start,end,initial_angle,final_angle);
	add_text (GTK_WIDGET(button),g_strdup_printf("\n-->Path Planning took:%gs", g_timer_elapsed( timer, NULL )));
    g_timer_destroy( timer );
	Planner->PrintNodeList();
	Planner->draw_path();
//	Planner->draw_tree();
	//draw_straight_path(GTK_WIDGET(button));
	gdk_beep();
}
void
on_simulate_radio_toggled              (GtkToggleButton *togglebutton,
                                        gpointer         user_data)
{
	platform = STAGE;
	k_distance = 15;			// Gain applied on Distance Displacement for path following controller
	k_theta = 28;				// Gain applied on Orientation Error for path following controller
	GtkWidget * w;
   	w = lookup_widget (GTK_WIDGET(togglebutton),"k_distance");
   	gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_distance));
   	w = lookup_widget (GTK_WIDGET(togglebutton),"k_theta");
   	gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_theta));
}


void
on_wheelchair_radio_toggled            (GtkToggleButton *togglebutton,
                                        gpointer         user_data)
{
	platform = WHEELCHAIR;
	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
	GtkWidget * w;
   	w = lookup_widget (GTK_WIDGET(togglebutton),"k_distance");
   	gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_distance));
   	w = lookup_widget (GTK_WIDGET(togglebutton),"k_theta");
   	gtk_entry_set_text((GtkEntry*) w,g_strdup_printf("%.2f", k_theta));
}
void
on_follow_path_released                (GtkButton       *button,
                                        gpointer         user_data)
{
	Node *temp_node;
	int i=0;
	temp_node = way_points;
	while(temp_node !=NULL)
	{
		//a = atan2(temp_node->next->location.y - temp_node->location.y  ,temp_node->next->location.x -temp_node->location.x);
		printf("\n Current Node is=%d and Location= X[%.3f],Y[%.3f] angle is=[%.3f]",++i,temp_node->location.x,temp_node->location.y,RTOD(temp_node->angle));
		fflush(stdout); 
		temp_node = temp_node->next;
	}
	if (!Planner->path)
	{
		add_text(GTK_WIDGET(button),g_strdup_printf("\n --->>> No Path to Follow <<<---"));
		return;
	}
	if(Follower)
	{
		Follower->path = Planner->path;
		Follower->kd = k_distance;
		Follower->kt = k_theta;
		Follower->ko = k_obstacle;
		Follower->tracking_distance = tracking_distance;
		Follower->log = 1;
	}
	Follower = new PathFollower(Planner->path,k_distance,k_theta,k_obstacle,tracking_distance,GTK_WIDGET(button),1,Planner,pixbuf);
	Follower->Connect(platform);
	Follower->stop = FALSE;
	Follower->FollowPath(Planner->path);
}
void
on_stop_following_released                (GtkButton       *button,
                                        gpointer         user_data)
{
	if(Follower)
		Follower->stop = TRUE;
};

void
on_window_exit                         (GtkObject       *object,
                                        gpointer         user_data)
{
if (Planner)
	{
	delete Planner;
	}
if (pixbuf)
	gdk_pixbuf_unref(pixbuf);
gtk_main_quit();
}


void
on_reg_grid_changed                    (GtkEditable     *editable,
                                        gpointer         user_data)
{
	reg_grid_distance = atof(gtk_editable_get_chars(editable,0,-1));
//	if(reg_grid_distance)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Arc Width changed to:%.3f", reg_grid_distance));
}


void
on_dist_node_changed                   (GtkEditable     *editable,
                                        gpointer         user_data)
{
	node_connection_distance = atof(gtk_editable_get_chars(editable,0,-1));
//	if(node_connection_distance)
		//add_text (GTK_WIDGET(editable),g_strdup_printf("\n->Arc Width changed to:%.3f", node_connection_distance));
}
void *print_message_function( thread_args *ptr );

void
on_connect_to_player_clicked           (GtkButton       *button,
                                        gpointer         user_data)
{
	Planner->draw_tree();
}

void *print_message_function( thread_args *ptr )
{
	for (int i=0;i<10;i++)
	{
		gdk_threads_enter(); //Thread Lock
     		g_usleep(100000);
     		printf("Kd =%.3f \n", ptr->kd);
     		g_usleep(100000);
     		printf("Kt =%.3f \n", ptr->kt);
     		g_usleep(100000);
     		printf("Tracking Distance=%.3f \n", ptr->tracking_distance);
		gdk_flush();
		//g_idle_add();
		gdk_threads_leave(); // Thread Release
	}
	g_thread_exit(g_thread_self());
return NULL;
}