Fawkes API
Fawkes Development Version
|
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