gtk_viewer1.c
Go to the documentation of this file.
00001 #include <stdlib.h>
00002 #include <goocanvas.h>
00003 #include "goo_laser_data.h"
00004 #include "gtk_viewer.h"
00005 
00006 static gboolean on_rect_button_press (GooCanvasItem  *view,
00007                                       GooCanvasItem  *target,
00008                                       GdkEventButton *event,
00009                                       gpointer        data);
00010 
00011 static gboolean on_delete_event      (GtkWidget      *window,
00012                                       GdkEvent       *event,
00013                                       gpointer        unused_data);
00014 
00015 void* reading_thread(void *data);
00016         
00017 GooCanvasItem*  text_item;
00018 GooCanvasItem* rect_item;
00019 
00020 int main (int argc, char **argv)
00021 {
00022         sm_set_program_name(basename(argv[0]));
00023         
00024         viewer_params *p = (viewer_params*) malloc(sizeof(viewer_params));
00025         lds_set_defaults(&(p->laser));
00026         ls_set_defaults(&(p->pose_path));
00027         
00028         p->laser.rays.draw = 0;
00029         p->laser.points.draw = 0;
00030         p->laser.normals.draw = 0;
00031         p->laser.countour.width = 0.1;
00032         p->pose_path.width = 0.1;
00033         p->pose_path.color = "#f00";
00034         
00035         struct option * ops = options_allocate(100);
00036         options_string(ops, "in", &(p->input_filename), "stdin", "input file (Carmen or JSON)");
00037         options_string(ops, "use", &(p->use), "estimate", "One in 'odometry','estimate','true_pose'");
00038         
00039         lds_add_options(&(p->laser), ops, "laser_", "");
00040         ls_add_options(&(p->pose_path), ops, "path_", "");
00041 
00042         if(!options_parse_args(ops, argc, argv)) {
00043                 fprintf(stderr, "A simple experimental GTK viewer.\n\nUsage:\n");
00044                 options_print_help(ops, stderr);
00045                 return -1;
00046         }
00047 
00048   /* init threads */
00049   g_thread_init(NULL);
00050   gdk_threads_init();
00051 
00052 
00053   GtkWidget *window, *scrolled_win, *canvas;
00054 GooCanvasItem *root;
00055 
00056   /* Initialize GTK+. */
00057   gtk_set_locale ();
00058   gtk_init (&argc, &argv);
00059 
00060   /* Create the window and widgets. */
00061   window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
00062   gtk_window_set_default_size (GTK_WINDOW (window), 640, 600);
00063   gtk_widget_show (window);
00064   g_signal_connect (window, "delete_event", (GtkSignalFunc) on_delete_event,
00065                     NULL);
00066 
00067   scrolled_win = gtk_scrolled_window_new (NULL, NULL);
00068   gtk_scrolled_window_set_shadow_type (GTK_SCROLLED_WINDOW (scrolled_win),
00069                                        GTK_SHADOW_IN);
00070   gtk_widget_show (scrolled_win);
00071   gtk_container_add (GTK_CONTAINER (window), scrolled_win);
00072 
00073   canvas = goo_canvas_new ();
00074 
00075   root = goo_canvas_get_root_item (GOO_CANVAS (canvas));
00076 
00077         p->device_size[0] = 800;
00078         p->device_size[1] = 600;
00079         
00080         gtk_widget_set_size_request (canvas, p->device_size[0], p->device_size[1]);
00081         goo_canvas_set_bounds (GOO_CANVAS (canvas), 0, 0, p->device_size[0], p->device_size[1]);
00082         gtk_widget_show (canvas);
00083         gtk_container_add (GTK_CONTAINER (scrolled_win), canvas);
00084 
00085         p->root = root;
00086         p->canvas = canvas;
00087         
00088         
00089         /* Add a few simple items. */
00090 rect_item = goo_canvas_rect_new (root, 0, 0, 50, 50,
00091                                    "line-width", 10.0,
00092                                    "radius-x", 20.0,
00093                                    "radius-y", 10.0,
00094                                    "stroke-color", "yellow",
00095                                    "fill-color", "red",
00096                                    NULL);
00097 
00098 /*text_item = goo_canvas_text_new (root, "Hello World", 300, 300, -1,
00099                                    GTK_ANCHOR_CENTER,
00100                                    "font", "Sans 1",
00101                                    NULL);
00102   goo_canvas_item_rotate (text_item, 45, 300, 300);
00103   */
00104 
00105         
00106         GError * error;
00107         if (!g_thread_create(reading_thread, p, FALSE, &error)) {
00108       g_printerr ("Failed to create YES thread: %s\n", error->message);
00109       return 1;
00110         }
00111   /* Pass control to the GTK+ main event loop. */
00112   gtk_main ();
00113 
00114 }
00115 
00116 void* reading_thread(void *data) {
00117         viewer_params * p = (viewer_params*) data;
00118         
00119         FILE * input = open_file_for_reading(p->input_filename);
00120         if(!input) return 0;
00121         
00122         p->scans = malloc(1);
00123         p->scans_items = malloc(1);
00124         p->scans_size = 0;
00125         p->scans_num = 0;
00126         
00127         LDP ld;
00128         while( (ld = ld_read_smart(input)) ) {
00129            
00130                 if(p->scans_num >= p->scans_size) {
00131                         p->scans_size = 2* p->scans_size +10;
00132                         p->scans = realloc(p->scans, sizeof(LDP) * p->scans_size);
00133                         p->scans_items = realloc(p->scans_items, sizeof(LDP) * p->scans_size);
00134                 }
00135                 
00136                 gdk_threads_enter();
00137 
00138                 if(1) {
00139                         GooCanvasItem * gld = goo_laser_data_new (p->root, p, ld);
00140                 g_signal_connect (gld, "button_press_event",
00141                             (GtkSignalFunc) on_rect_button_press, NULL);
00142                         p->scans[p->scans_num] = ld;
00143                         p->scans_items[p->scans_num] = gld;
00144                         p->scans_num++;
00145                 } else {
00146 
00147                 }
00148                 compute_transformations(p);
00149                 goo_canvas_update(p->canvas);
00150 /*              gdk_flush ();*/
00151                 
00152                 gdk_threads_leave();
00153                 /* sleep a while */
00154                 usleep(20);
00155 /*            sleep(g_random_int_range (1, 4));*/
00156         }
00157 
00158   return 0;
00159 }
00160 
00161 void world_to_viewport(
00162         const oriented_bbox*obbox, const double device_size[2], cairo_matrix_t*m) {
00163         
00164         cairo_matrix_init_identity(m);
00165         
00166         double scale[2] = { 
00167                 device_size[0] / obbox->size[0], 
00168                 device_size[1] / obbox->size[1]};
00169                 
00170         double s = scale[0] < scale[1] ? scale[0] : scale[1];
00171         cairo_matrix_scale(m, s, s);
00172         cairo_matrix_scale(m, 1, -1);
00173         cairo_matrix_rotate(m, -obbox->pose[2]);        
00174         cairo_matrix_translate(m, -obbox->pose[0], -obbox->pose[1]);
00175         
00176 }
00177 
00178 void item_to_world(const double pose[3], cairo_matrix_t*m) {
00179         cairo_matrix_init_identity(m);
00180         cairo_matrix_translate(m, pose[0], pose[1]);
00181         cairo_matrix_rotate(m, pose[2]);        
00182 }
00183 
00184 void compute_transformations(viewer_params*p) {
00185         int k;
00186         oriented_bbox global;
00187         
00188         if(0) {
00189                 bbfind * bbf = bbfind_new();
00190                 int n = p->scans_num;
00191 /*              if(n>20) n = 20;*/
00192         
00193                 for(k=0;k<n;k++) {
00194                         GooLaserData * gld = (GooLaserData*) p->scans_items[k];
00195                         bbfind_add_bbox(bbf, &gld->obbox);
00196                 }
00197         
00198         
00199                 if(bbfind_compute(bbf, &global)) {
00200                         sm_debug("%d Global: %s size %f %f\n", p->scans_num, friendly_pose(global.pose), 
00201                                 global.size[0], global.size[1]);
00202                 } else {
00203                         sm_error("%d Could not compute global bounding box.\n", p->scans_num);
00204                 }
00205                 bbfind_free(bbf);
00206         }else{
00207         
00208         global.pose[0] = -22;
00209         global.pose[1] = -41;
00210         global.pose[2] = M_PI/2;
00211         global.size[0] = 106;
00212         global.size[1] = 46;
00213 }
00214         cairo_matrix_t m_world_to_viewport;
00215         world_to_viewport(&global, p->device_size, &(m_world_to_viewport));
00216 
00217         cairo_matrix_t *m = &m_world_to_viewport;
00218         sm_info("Matrix: %f %f %f %f  %f %f\n", m->xx,m->yx,m->xy,m->yy, m->x0, m->y0);
00219 
00220         cairo_matrix_t mm;
00221         cairo_matrix_init_identity(&mm);
00222 /*      cairo_matrix_translate(&mm, 300, 600);*/
00223         cairo_matrix_translate(&mm, 300, 600);
00224         cairo_matrix_rotate(&mm, deg2rad(p->scans_num));
00225         goo_canvas_item_set_transform(rect_item, &mm);
00226 
00227         /*
00228         goo_canvas_item_set_transform(text_item, &m_world_to_viewport);
00229 */
00230         for(k=0;k<p->scans_num;k++) {
00231                 GooCanvasItem * gld = p->scans_items[k];
00232                 
00233                 double * pose = p->scans[k]->estimate;
00234                 cairo_matrix_t m_item_to_world;
00235                 item_to_world(pose, &m_item_to_world);
00236 
00237                 cairo_matrix_t transform;
00238                 cairo_matrix_multiply(&transform, &m_item_to_world, &m_world_to_viewport);
00239                 goo_canvas_item_set_transform(gld, &transform);
00240         }
00241 }
00242 
00243 /* This handles button presses in item views. We simply output a message to
00244    the console. */
00245 static gboolean
00246 on_rect_button_press (GooCanvasItem  *item,
00247                       GooCanvasItem  *target,
00248                       GdkEventButton *event,
00249                       gpointer        data)
00250 {
00251   g_print ("rect item received button press event\n");
00252   return TRUE;
00253 }
00254 
00255 
00256 /* This is our handler for the "delete-event" signal of the window, which
00257    is emitted when the 'x' close button is clicked. We just exit here. */
00258 static gboolean
00259 on_delete_event (GtkWidget *window,
00260                  GdkEvent  *event,
00261                  gpointer   unused_data)
00262 {
00263   exit (0);
00264 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29