mcl_gtk.c

/*
 * @(#)mcl_gtk.c       07/04/06
 * 
 * Description  : GTK animation for a simple implementation of MCL 
 *                (MCL - Monte Carlo Localization).
 * License      : GNU GPL v.2
 * Author       : Tran Duy Khanh (www.tran.cz)
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <robomath.h>
#include <mcl.h>
#include "mcl_gtk.h"

struct mcl_model mcl;

void print_usage()
{
        fprintf(file_err, "A simple GTK demonstration of the Monte Carlo "
                        "algorithm,\n"
                        "a method used for mobile robot localization.\n"
                        "GNU GPL v.2, CTU FEE in Prague - Department of "
                        "Control Engineering\n\n");
        fprintf(file_err, "Usage :  [options]\n\n");
        fprintf(file_err, "    -g\t\tTest the GTK graphic part\n");
        fprintf(file_err, "    -h\t\tThis help\n");
        fprintf(file_err, "    -r\t\tTest the random generator\n");
        fprintf(file_err, "    -t\t\tText mode only\n");
        fprintf(file_err, "\n");

        exit(0);
}

int parseopt(int argc, char **argv)
{
        int ch;
        
        file_out = stdout;
        file_err = stderr;

        optind = 0;
        /* parse the program arguments */
        while ((ch = getopt(argc, argv, "ghrt")) != -1) {
                switch(ch) {
                        /* test GTK graphic part */
                        case 'g':       option.opt_flag |= OPT_GTK_TEST;
                                        test_animation(argc, argv);
                                        break;
                        /* print program usage */
                        case 'h':       print_usage(); break;
                        /* test random generator */
                        case 'r':       testrand(100); exit(0);
                                        break;
                        /* text output only. No GTK. */
                        case 't':       option.opt_flag |= OPT_TEXT_MODE;
                                        break;
                        default:        print_usage(); break;
                }
        }
        
        return 0;
}

/****************************************************************************
 * MCL stuffs.
 ****************************************************************************
 */
void init_robot(struct mcl_model *mcl)
{
        struct mcl_particle *robot;

        /* the robot particle is used just to be able to move the robot on
         * the playground. */
        mcl->system = (struct mcl_particle*)
                        malloc(sizeof(struct mcl_particle));
        robot = mcl->system;
        robot->x = 216;
        robot->y = 300;
        robot->angle = DEGREES(0);;
        robot->weight = 1;
}

void init_mcl(struct mcl_model *mcl)
{
        /* the noises */
        mcl->gen_dnoise = 0.99;         /* distance noise */
        mcl->gen_anoise = DEGREES(10);  /* angle noise */
        mcl->mov_dnoise = 1.1;
        mcl->mov_anoise = 2.0;
        mcl->w_min = 0.25;
        mcl->w_max = 2.0;
        mcl->eval_sigma = 0.5;
        mcl->maxavdist = 50;
        mcl->maxnoisecycle = 10;        /* bad cycles before reseting */

        /* amount of particles */
        mcl->count = 5000;
        mcl->parts = (struct mcl_particle *)
                        malloc(sizeof(struct mcl_particle)*mcl->count);

        /* phases of the MCL */
        mcl->init = mcl_init;
        mcl->move = mcl_move;
        mcl->update = mcl_update;
        mcl->normalize = mcl_normalize;
        mcl->sort = mcl_partsort;
        mcl->resample = mcl_resample;

        /* cycles of enumeration */
        mcl->cycle = 0;
        mcl->noisecycle = 0;

        /* generate particles with noises */
        mcl->init(mcl);
}

void move(struct mcl_model *mcl, double dx, double dy, double dangle)
{
        struct mcl_particle *robot = (struct mcl_particle *)mcl->system;
        struct mcl_particle x;

        mcl->cycle++;
        printf("cycle = %d\n", mcl->cycle);
        /* move particles */
        mcl->move(mcl, dx, dy, dangle);

        /* the robot particle is used just to be able to move the robot on
         * the playground. In addition we use this values as the reference 
         * values measured by sensors. In the real world, the sensors measure
         * with a certain noise, but int this example we consider the 
         * measurements are not suffered by noises. */
        robot->x += dx*cos(robot->angle) + dy*sin(robot->angle);
        robot->y += dy*cos(robot->angle) + dx*sin(robot->angle);
        robot->angle += dangle;

        /* normally, the [x,y,angle] are values measured by sensors. In this 
         * example, the values are quantified as the movement without noises*/
        mcl->update(mcl, robot->x, robot->y, robot->angle);
        if (mcl->flag == MCL_RESET) {
                init_mcl(mcl);
                mcl->flag = MCL_RUN;
                return;
        }
        mcl->normalize(mcl);
        mcl->sort(mcl);
        mcl->resample(mcl);
        x = mcl_get_pos(mcl);
        printf("estimated position: %f %f %f %f\n", x.x, x.y, x.angle, x.weight);
}
/* end of the MCL stuffs.
 ****************************************************************************/

/****************************************************************************
 * GTK Graphic stuffs.
 ****************************************************************************
 */

/* Get out of the aplication */
void quit ()
{
        gtk_exit (0);
}

/*void get_size (GdkPixmap *xpm, unsigned int *width, unsigned int *height)
{
}*/

void load_pixmaps (GtkWidget *widget, struct moveable_obj *moveable, 
                                    struct pixmap_obj *pixmaps)
{
        int i = 0;
        
        /* Get every sequence */
        while (pixmaps[i].xpm_filename) {
                /* Convert xpm data to pixmap & mask */
                pixmaps[i].pixmap = gdk_pixmap_create_from_xpm(widget->window, 
                                        NULL, NULL, pixmaps[i].xpm_filename);
                i++;
        }

        /* Get sprite height and width */
        /* get_size(pixmaps[0].pixmap, &pixmaps[0].width, &pixmaps[0].height); */
        
        /* Initialize sprite information */
        pixmaps->seq_count = i;
}

void load_scene_pixmap(GtkWidget *widget)
{
        /* load background, robot, sample pixmaps */
        load_pixmaps(widget, moveable, xpm_background);
        load_pixmaps(widget, moveable, xpm_robot);
        load_pixmaps(widget, moveable, xpm_sample);
}

static gboolean key_event(GtkWidget *widget, GdkEventKey *event)
{
        GdkModifierType state;

        switch (event->keyval) {
                /* left */
                case 65361:
                        move(&mcl, -2.0, 0.0, 0.0);
                        break;
                /* right */
                case 65363:
                        move(&mcl, 2.0, 0.0, 0.0);
                        break;
                /* up */
                case 65362:
                        move(&mcl, 0.0, 2.0, 0.0);
                        break;
                /* down */
                case 65364:
                        move(&mcl, 0.0, -2.0, 0.0);
                        break;
                /* enter - reinitialization */
                case 65293:
                        init_mcl(&mcl);
                        break;
                /* quit */
                case 'q':
                case 'Q':
                case 65307:
                        quit();
        }
        
        return TRUE;
}

static gboolean button_event(GtkWidget *widget, GdkEventMotion *event)
{
        int x, y;
        GdkModifierType state;
        struct mcl_particle *robot = (struct mcl_particle *)mcl.system;

        /* get the position mouse state */
        if (event->is_hint) 
                gdk_window_get_pointer (event->window, &x, &y, &state);
        else {
                x = event->x;
                y = event->y;
                state = event->state;
        }
                printf("x=%d y=%d\n", x, y);

        /* replace robot to another position */
        if (state & GDK_BUTTON3_MASK) {
                robot->x = (int)(x*FWIDTH/632);
                robot->y = (int)(FHEIGHT-(y*FHEIGHT/452));
        /* move particles and reset if needed */
        } else {
                mcl.cycle++;
                printf("cycle = %d\n", mcl.cycle);
                /* move particles */
                mcl.move(&mcl, 2.0, 2.0, 0.0);
                mcl.update(&mcl, x-BORDER, y-BORDER, 0);
                if (mcl.flag == MCL_RESET) {
                        init_mcl(&mcl);
                        mcl.flag = MCL_RUN;
                        return TRUE;
                }
                mcl.normalize(&mcl);
                mcl.sort(&mcl);
                mcl.resample(&mcl);
        }
        
        return TRUE;
}

static gboolean motion_notify_event( GtkWidget *widget, GdkEventMotion *event)
{
        int x, y;
        GdkModifierType state;

        if (event->is_hint) 
                gdk_window_get_pointer (event->window, &x, &y, &state);
        else {
                x = event->x;
                y = event->y;
                state = event->state;
        }
  
        return TRUE;
}

static gint configure_event (GtkWidget *widget, GdkEventConfigure *event)
{
        /* Free background if we created it */
        if (xpm_scene) {
                gdk_pixmap_unref (xpm_scene);
        } 

        /* Create a new pixmap with new size */
        xpm_scene = gdk_pixmap_new (widget->window,
                        widget->allocation.width,
                        widget->allocation.height,
                        -1);
        
        return TRUE;
}

gint expose_event (GtkWidget *widget, GdkEventExpose *event)
{
        int i;
        
        /* Copy pixmap to the window */
        gdk_draw_pixmap (widget->window,
                widget->style->fg_gc[GTK_WIDGET_STATE (widget)],
                xpm_scene,
                event->area.x, event->area.y,
                event->area.x, event->area.y,
                event->area.width, event->area.height);
        
        return FALSE;
}

void (*repaint_objects)();

gint repaint (gpointer data)
{
        GtkWidget  *drawing_area = (GtkWidget *)data;
        GdkRectangle update_rect;
        
        /*test_repaint();*/
        (*repaint_objects)();
        
        /* The whole screen needs to be updated */
        update_rect.x = 0;
        update_rect.y = 0;
        update_rect.width = drawing_area->allocation.width;
        update_rect.height = drawing_area->allocation.height;
        
        /* So update it */
        gtk_widget_draw (drawing_area, &update_rect);
}

void test_repaint()
{
        int i;
        
        /* clear pixmap (background image) */
        gdk_draw_drawable (xpm_scene,
                drawing_area->style->fg_gc[GTK_WIDGET_STATE (drawing_area)],
                xpm_background[0].pixmap,
                0, 0,
                0, 0,
                drawing_area->allocation.width,
                drawing_area->allocation.height);
}

void init_animation(int argc, char *argv[], void (*repaint_func)())
{
        /* Initialize GTK */
        gtk_init (&argc, &argv);
        
        /* Create the top level window */
        window = gtk_window_new (GTK_WINDOW_TOPLEVEL);
        
        gtk_signal_connect (GTK_OBJECT (window), "destroy",
                                GTK_SIGNAL_FUNC (quit), NULL);
        
        /* Create the drawing area */
        drawing_area = gtk_drawing_area_new ();
        gtk_drawing_area_size (GTK_DRAWING_AREA (drawing_area), 
                                xpm_background[0].width, 
                                xpm_background[0].height);
        gtk_container_add (GTK_CONTAINER (window), drawing_area);

        gtk_signal_connect (GTK_OBJECT (drawing_area), "expose_event",
                                (GtkSignalFunc) expose_event, NULL);
        gtk_signal_connect (GTK_OBJECT(drawing_area),"configure_event",
                                (GtkSignalFunc) configure_event, NULL);
        gtk_signal_connect (GTK_OBJECT (drawing_area), "motion_notify_event",
                                (GtkSignalFunc) motion_notify_event, NULL);
        gtk_signal_connect (GTK_OBJECT(drawing_area),"button_press_event",
                                (GtkSignalFunc) button_event, NULL);
        gtk_signal_connect (GTK_OBJECT(drawing_area),"button_release_event",
                                (GtkSignalFunc) button_event, NULL);
        gtk_signal_connect (GTK_OBJECT(window),"key_press_event",
                                (GtkSignalFunc) key_event, NULL);


        gtk_widget_set_events (drawing_area, GDK_EXPOSURE_MASK
                                | GDK_LEAVE_NOTIFY_MASK
                                | GDK_BUTTON_PRESS_MASK
                                | GDK_POINTER_MOTION_MASK
                                | GDK_POINTER_MOTION_HINT_MASK);

        gtk_widget_show(drawing_area);
        gtk_widget_show(window);

        /* Repaint every so often */
        gtk_timeout_add (SCENE_REFRESH, repaint, drawing_area);
        repaint_objects = repaint_func;
        
        /* Load all scene pixmaps */
        load_scene_pixmap(window);
        
        /* Call gtk-main loop */
        gtk_main();
}

int get_color(double weight)
{
        if (weight <= 0.005)
                return 0;
        else if (weight <= 0.01)
                return 1;
        else if (weight <= 0.1)
                return 2;
        else if (weight <= 0.5)
                return 3;
        else
                return 4;
}

void test_animation(int argc, char *argv[])
{
        init_animation(argc, argv, test_repaint);
}

void scene_repaint()
{
        struct mcl_particle *parts = (struct mcl_particle *)mcl.parts;
        struct mcl_particle *robot = (struct mcl_particle *)mcl.system;
        int i;
        int x;
        
        /* clear pixmap (background image) */
        gdk_draw_drawable (xpm_scene,
                drawing_area->style->fg_gc[GTK_WIDGET_STATE (drawing_area)],
                xpm_background[0].pixmap,
                0, 0,
                0, 0,
                drawing_area->allocation.width,
                drawing_area->allocation.height);

        /* draw the robot */
        gdk_draw_drawable (xpm_scene,
                drawing_area->style->fg_gc[GTK_WIDGET_STATE (drawing_area)],
                xpm_robot[0].pixmap,
                0, 0,
                (int)(robot->x/FWIDTH*(632-2*BORDER)+BORDER) 
                                        - xpm_robot[0].height/2,
                (int)((FHEIGHT-robot->y)/FHEIGHT*(452-2*BORDER)+BORDER) 
                                        - xpm_robot[0].width/2, 
                drawing_area->allocation.width,
                drawing_area->allocation.height);

        /* draw the particles */
        for (i=0; i<mcl.count; i++) {
                gdk_draw_drawable (xpm_scene,
                        drawing_area->style->fg_gc[GTK_WIDGET_STATE (drawing_area)],
                        xpm_sample[get_color(parts[i].weight)].pixmap,
                        0, 0,
                        (int)(parts[i].x/FWIDTH*(632-2*BORDER)+BORDER) 
                                                - xpm_sample[0].height/2,
                        (int)((FHEIGHT-parts[i].y)/FHEIGHT*(452-2*BORDER)+BORDER) 
                                                - xpm_sample[0].width/2, 
                        drawing_area->allocation.width,
                        drawing_area->allocation.height);
        }                                
}

/* end of the GTK stuffs.
 ****************************************************************************/

void initdata()
{
        /* init mutex */
        pthread_mutex_init(&mcl_mutex, NULL);

        /* MCL initialization */
        mcl.width = FWIDTH;
        mcl.height = FHEIGHT;
        /* the robot particle is used just to be able to move the robot on
         * the playground. */
        init_robot(&mcl);
        init_mcl(&mcl);

        /*
        pthread_mutex_lock(&mcl_mutex);
        pthread_mutex_unlock(&mcl_mutex);*/
}

void destroydata()
{
        pthread_mutex_destroy(&mcl_mutex);
}

int main(int _argc, char **_argv)
{
        argc = _argc;
        argv = _argv;
        
        /* parse program options */
        parseopt(argc, argv);
        
        /* data initialization */
        initdata();

        if (!(option.opt_flag & OPT_TEXT_MODE)) {
                /* run animation initialization */
                init_animation(argc, argv, scene_repaint);
        }
        
        /* destroy data (mutex, ...) */
        destroydata();
        
        return 0;
}

---