Fawkes API  Fawkes Development Version
map_draw.c
1 
2 /***************************************************************************
3  * map_draw.c: Local map GUI functions
4  *
5  * Created: Thu May 24 18:46:58 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: Local map GUI functions
31  * Author: Andrew Howard
32  * Date: 18 Jan 2003
33 **************************************************************************/
34 
35 #ifdef INCLUDE_RTKGUI
36 
37 #include <errno.h>
38 #include <math.h>
39 #include <stdlib.h>
40 #include <string.h>
41 
42 #include <rtk.h>
43 #include "map.h"
44 
45 /// @cond EXTERNAL
46 
47 ////////////////////////////////////////////////////////////////////////////
48 // Draw the occupancy map
49 void map_draw_occ(map_t *map, rtk_fig_t *fig)
50 {
51  int i, j;
52  int col;
53  map_cell_t *cell;
54  uint16_t *image;
55  uint16_t *pixel;
56 
57  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
58 
59  // Draw occupancy
60  for (j = 0; j < map->size_y; j++)
61  {
62  for (i = 0; i < map->size_x; i++)
63  {
64  cell = map->cells + MAP_INDEX(map, i, j);
65  pixel = image + (j * map->size_x + i);
66 
67  col = 127 - 127 * cell->occ_state;
68  *pixel = RTK_RGB16(col, col, col);
69  }
70  }
71 
72  // Draw the entire occupancy map as an image
73  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
74  map->scale, map->size_x, map->size_y, 16, image, NULL);
75 
76  free(image);
77 
78  return;
79 }
80 
81 
82 ////////////////////////////////////////////////////////////////////////////
83 // Draw the cspace map
84 void map_draw_cspace(map_t *map, rtk_fig_t *fig)
85 {
86  int i, j;
87  int col;
88  map_cell_t *cell;
89  uint16_t *image;
90  uint16_t *pixel;
91 
92  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
93 
94  // Draw occupancy
95  for (j = 0; j < map->size_y; j++)
96  {
97  for (i = 0; i < map->size_x; i++)
98  {
99  cell = map->cells + MAP_INDEX(map, i, j);
100  pixel = image + (j * map->size_x + i);
101 
102  col = 255 * cell->occ_dist / map->max_occ_dist;
103 
104  *pixel = RTK_RGB16(col, col, col);
105  }
106  }
107 
108  // Draw the entire occupancy map as an image
109  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
110  map->scale, map->size_x, map->size_y, 16, image, NULL);
111 
112  free(image);
113 
114  return;
115 }
116 
117 
118 ////////////////////////////////////////////////////////////////////////////
119 // Draw a wifi map
120 void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
121 {
122  int i, j;
123  int level, col;
124  map_cell_t *cell;
125  uint16_t *image, *mask;
126  uint16_t *ipix, *mpix;
127 
128  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
129  mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
130 
131  // Draw wifi levels
132  for (j = 0; j < map->size_y; j++)
133  {
134  for (i = 0; i < map->size_x; i++)
135  {
136  cell = map->cells + MAP_INDEX(map, i, j);
137  ipix = image + (j * map->size_x + i);
138  mpix = mask + (j * map->size_x + i);
139 
140  level = cell->wifi_levels[index];
141 
142  if (cell->occ_state == -1 && level != 0)
143  {
144  col = 255 * (100 + level) / 100;
145  *ipix = RTK_RGB16(col, col, col);
146  *mpix = 1;
147  }
148  else
149  {
150  *mpix = 0;
151  }
152  }
153  }
154 
155  // Draw the entire occupancy map as an image
156  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
157  map->scale, map->size_x, map->size_y, 16, image, mask);
158 
159  free(mask);
160  free(image);
161 
162  return;
163 }
164 
165 /// @endcond
166 
167 #endif