map_draw.c
Go to the documentation of this file.
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00004  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00005  *
00006  *  This library is free software; you can redistribute it and/or
00007  *  modify it under the terms of the GNU Lesser General Public
00008  *  License as published by the Free Software Foundation; either
00009  *  version 2.1 of the License, or (at your option) any later version.
00010  *
00011  *  This library is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  *  Lesser General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU Lesser General Public
00017  *  License along with this library; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /**************************************************************************
00022  * Desc: Local map GUI functions
00023  * Author: Andrew Howard
00024  * Date: 18 Jan 2003
00025  * CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
00026 **************************************************************************/
00027 
00028 #ifdef INCLUDE_RTKGUI
00029 
00030 #include <errno.h>
00031 #include <math.h>
00032 #include <stdlib.h>
00033 #include <string.h>
00034 
00035 #include <rtk.h>
00036 #include "nav2d_localizer/map.h"
00037 
00038 
00040 // Draw the occupancy map
00041 void map_draw_occ(map_t *map, rtk_fig_t *fig)
00042 {
00043   int i, j;
00044   int col;
00045   map_cell_t *cell;
00046   uint16_t *image;
00047   uint16_t *pixel;
00048 
00049   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00050 
00051   // Draw occupancy
00052   for (j = 0; j < map->size_y; j++)
00053   {
00054     for (i =  0; i < map->size_x; i++)
00055     {
00056       cell = map->cells + MAP_INDEX(map, i, j);
00057       pixel = image + (j * map->size_x + i);
00058 
00059       col = 127 - 127 * cell->occ_state;
00060       *pixel = RTK_RGB16(col, col, col);
00061     }
00062   }
00063 
00064   // Draw the entire occupancy map as an image
00065   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00066                 map->scale, map->size_x, map->size_y, 16, image, NULL);
00067 
00068   free(image);
00069 
00070   return;
00071 }
00072 
00073 
00075 // Draw the cspace map
00076 void map_draw_cspace(map_t *map, rtk_fig_t *fig)
00077 {
00078   int i, j;
00079   int col;
00080   map_cell_t *cell;
00081   uint16_t *image;
00082   uint16_t *pixel;
00083 
00084   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00085 
00086   // Draw occupancy
00087   for (j = 0; j < map->size_y; j++)
00088   {
00089     for (i =  0; i < map->size_x; i++)
00090     {
00091       cell = map->cells + MAP_INDEX(map, i, j);
00092       pixel = image + (j * map->size_x + i);
00093 
00094       col = 255 * cell->occ_dist / map->max_occ_dist;
00095 
00096       *pixel = RTK_RGB16(col, col, col);
00097     }
00098   }
00099 
00100   // Draw the entire occupancy map as an image
00101   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00102                 map->scale, map->size_x, map->size_y, 16, image, NULL);
00103 
00104   free(image);
00105 
00106   return;
00107 }
00108 
00109 
00111 // Draw a wifi map
00112 void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
00113 {
00114   int i, j;
00115   int level, col;
00116   map_cell_t *cell;
00117   uint16_t *image, *mask;
00118   uint16_t *ipix, *mpix;
00119 
00120   image = malloc(map->size_x * map->size_y * sizeof(image[0]));
00121   mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
00122 
00123   // Draw wifi levels
00124   for (j = 0; j < map->size_y; j++)
00125   {
00126     for (i =  0; i < map->size_x; i++)
00127     {
00128       cell = map->cells + MAP_INDEX(map, i, j);
00129       ipix = image + (j * map->size_x + i);
00130       mpix = mask + (j * map->size_x + i);
00131 
00132       level = cell->wifi_levels[index];
00133 
00134       if (cell->occ_state == -1 && level != 0)
00135       {
00136         col = 255 * (100 + level) / 100;
00137         *ipix = RTK_RGB16(col, col, col);
00138         *mpix = 1;
00139       }
00140       else
00141       {
00142         *mpix = 0;
00143       }
00144     }
00145   }
00146 
00147   // Draw the entire occupancy map as an image
00148   rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
00149                 map->scale, map->size_x, map->size_y, 16, image, mask);
00150 
00151   free(mask);
00152   free(image);
00153 
00154   return;
00155 }
00156 
00157 
00158 #endif


nav2d_localizer
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:52:57