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 
56  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
57 
58  // Draw occupancy
59  for (j = 0; j < map->size_y; j++)
60  {
61  for (i = 0; i < map->size_x; i++)
62  {
63  uint16_t *pixel = image + (j * map->size_x + i);
64  cell = map->cells + MAP_INDEX(map, i, j);
65 
66  col = 127 - 127 * cell->occ_state;
67  *pixel = RTK_RGB16(col, col, col);
68  }
69  }
70 
71  // Draw the entire occupancy map as an image
72  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
73  map->scale, map->size_x, map->size_y, 16, image, NULL);
74 
75  free(image);
76 
77  return;
78 }
79 
80 
81 ////////////////////////////////////////////////////////////////////////////
82 // Draw the cspace map
83 void map_draw_cspace(map_t *map, rtk_fig_t *fig)
84 {
85  int i, j;
86  uint16_t *image;
87 
88  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
89 
90  // Draw occupancy
91  for (j = 0; j < map->size_y; j++)
92  {
93  for (i = 0; i < map->size_x; i++)
94  {
95  map_cell_t *cell = map->cells + MAP_INDEX(map, i, j);
96  uint16_t *pixel = image + (j * map->size_x + i);
97 
98  int col = 255 * cell->occ_dist / map->max_occ_dist;
99 
100  *pixel = RTK_RGB16(col, col, col);
101  }
102  }
103 
104  // Draw the entire occupancy map as an image
105  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
106  map->scale, map->size_x, map->size_y, 16, image, NULL);
107 
108  free(image);
109 
110  return;
111 }
112 
113 
114 ////////////////////////////////////////////////////////////////////////////
115 // Draw a wifi map
116 void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
117 {
118  int i, j;
119  int level, col;
120  map_cell_t *cell;
121  uint16_t *image, *mask;
122 
123  image = malloc(map->size_x * map->size_y * sizeof(image[0]));
124  mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
125 
126  // Draw wifi levels
127  for (j = 0; j < map->size_y; j++)
128  {
129  for (i = 0; i < map->size_x; i++)
130  {
131  uint16_t *ipix = image + (j * map->size_x + i);
132  uint16_t *mpix = mask + (j * map->size_x + i);
133  cell = map->cells + MAP_INDEX(map, i, j);
134 
135  level = cell->wifi_levels[index];
136 
137  if (cell->occ_state == -1 && level != 0)
138  {
139  col = 255 * (100 + level) / 100;
140  *ipix = RTK_RGB16(col, col, col);
141  *mpix = 1;
142  }
143  else
144  {
145  *mpix = 0;
146  }
147  }
148  }
149 
150  // Draw the entire occupancy map as an image
151  rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
152  map->scale, map->size_x, map->size_y, 16, image, mask);
153 
154  free(mask);
155  free(image);
156 
157  return;
158 }
159 
160 /// @endcond
161 
162 #endif