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
49void 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
83void 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
116void 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