Fawkes API  Fawkes Development Version
map_draw.c
00001 
00002 /***************************************************************************
00003  *  map_draw.c: Local map GUI functions
00004  *
00005  *  Created: Thu May 24 18:46:58 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 /**************************************************************************
00030  * Desc: Local map GUI functions
00031  * Author: Andrew Howard
00032  * Date: 18 Jan 2003
00033 **************************************************************************/
00034 
00035 #ifdef INCLUDE_RTKGUI
00036 
00037 #include <errno.h>
00038 #include <math.h>
00039 #include <stdlib.h>
00040 #include <string.h>
00041 
00042 #include <rtk.h>
00043 #include "map.h"
00044 
00045 /// @cond EXTERNAL
00046 
00047 ////////////////////////////////////////////////////////////////////////////
00048 // Draw the occupancy map
00049 void map_draw_occ(map_t *map, rtk_fig_t *fig)
00050 {
00051   int i, j;
00052   int col;
00053   map_cell_t *cell;
00054   uint16_t *image;
00055   uint16_t *pixel;
00056 
00057   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00058 
00059   // Draw occupancy
00060   for (j = 0; j < map->size_y; j++)
00061   {
00062     for (i =  0; i < map->size_x; i++)
00063     {
00064       cell = map->cells + MAP_INDEX(map, i, j);
00065       pixel = image + (j * map->size_x + i);
00066 
00067       col = 127 - 127 * cell->occ_state;
00068       *pixel = RTK_RGB16(col, col, col);
00069     }
00070   }
00071 
00072   // Draw the entire occupancy map as an image
00073   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00074                 map->scale, map->size_x, map->size_y, 16, image, NULL);
00075 
00076   free(image);
00077 
00078   return;
00079 }
00080 
00081 
00082 ////////////////////////////////////////////////////////////////////////////
00083 // Draw the cspace map
00084 void map_draw_cspace(map_t *map, rtk_fig_t *fig)
00085 {
00086   int i, j;
00087   int col;
00088   map_cell_t *cell;
00089   uint16_t *image;
00090   uint16_t *pixel;
00091 
00092   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00093 
00094   // Draw occupancy
00095   for (j = 0; j < map->size_y; j++)
00096   {
00097     for (i =  0; i < map->size_x; i++)
00098     {
00099       cell = map->cells + MAP_INDEX(map, i, j);
00100       pixel = image + (j * map->size_x + i);
00101 
00102       col = 255 * cell->occ_dist / map->max_occ_dist;
00103 
00104       *pixel = RTK_RGB16(col, col, col);
00105     }
00106   }
00107 
00108   // Draw the entire occupancy map as an image
00109   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00110                 map->scale, map->size_x, map->size_y, 16, image, NULL);
00111 
00112   free(image);
00113 
00114   return;
00115 }
00116 
00117 
00118 ////////////////////////////////////////////////////////////////////////////
00119 // Draw a wifi map
00120 void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
00121 {
00122   int i, j;
00123   int level, col;
00124   map_cell_t *cell;
00125   uint16_t *image, *mask;
00126   uint16_t *ipix, *mpix;
00127 
00128   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00129   mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
00130 
00131   // Draw wifi levels
00132   for (j = 0; j < map->size_y; j++)
00133   {
00134     for (i =  0; i < map->size_x; i++)
00135     {
00136       cell = map->cells + MAP_INDEX(map, i, j);
00137       ipix = image + (j * map->size_x + i);
00138       mpix = mask + (j * map->size_x + i);
00139 
00140       level = cell->wifi_levels[index];
00141 
00142       if (cell->occ_state == -1 && level != 0)
00143       {
00144         col = 255 * (100 + level) / 100;
00145         *ipix = RTK_RGB16(col, col, col);
00146         *mpix = 1;
00147       }
00148       else
00149       {
00150         *mpix = 0;
00151       }
00152     }
00153   }
00154 
00155   // Draw the entire occupancy map as an image
00156   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00157                 map->scale, map->size_x, map->size_y, 16, image, mask);
00158 
00159   free(mask);
00160   free(image);
00161 
00162   return;
00163 }
00164 
00165 /// @endcond
00166 
00167 #endif