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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:09