X-lanni
148 lines
4.0 KiB
C
148 lines
4.0 KiB
C
/*
|
|
* Player - One Hell of a Robot Server
|
|
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
|
|
* gerkey@usc.edu kaspers@robotics.usc.edu
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*
|
|
*/
|
|
/**************************************************************************
|
|
* Desc: Local map GUI functions
|
|
* Author: Andrew Howard
|
|
* Date: 18 Jan 2003
|
|
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
|
|
**************************************************************************/
|
|
|
|
#pragma GCC diagnostic ignored "-Wpedantic"
|
|
#ifdef INCLUDE_RTKGUI
|
|
|
|
#include <errno.h>
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include <rtk.h>
|
|
#include "nav2_amcl/map/map.hpp"
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// Draw the occupancy map
|
|
void map_draw_occ(map_t * map, rtk_fig_t * fig)
|
|
{
|
|
int i, j;
|
|
int col;
|
|
map_cell_t * cell;
|
|
uint16_t * image;
|
|
uint16_t * pixel;
|
|
|
|
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
|
|
|
// Draw occupancy
|
|
for (j = 0; j < map->size_y; j++) {
|
|
for (i = 0; i < map->size_x; i++) {
|
|
cell = map->cells + MAP_INDEX(map, i, j);
|
|
pixel = image + (j * map->size_x + i);
|
|
|
|
col = 127 - 127 * cell->occ_state;
|
|
*pixel = RTK_RGB16(col, col, col);
|
|
}
|
|
}
|
|
|
|
// Draw the entire occupancy map as an image
|
|
rtk_fig_image(
|
|
fig, map->origin_x, map->origin_y, 0,
|
|
map->scale, map->size_x, map->size_y, 16, image, NULL);
|
|
|
|
free(image);
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// Draw the cspace map
|
|
void map_draw_cspace(map_t * map, rtk_fig_t * fig)
|
|
{
|
|
int i, j;
|
|
int col;
|
|
map_cell_t * cell;
|
|
uint16_t * image;
|
|
uint16_t * pixel;
|
|
|
|
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
|
|
|
// Draw occupancy
|
|
for (j = 0; j < map->size_y; j++) {
|
|
for (i = 0; i < map->size_x; i++) {
|
|
cell = map->cells + MAP_INDEX(map, i, j);
|
|
pixel = image + (j * map->size_x + i);
|
|
|
|
col = 255 * cell->occ_dist / map->max_occ_dist;
|
|
|
|
*pixel = RTK_RGB16(col, col, col);
|
|
}
|
|
}
|
|
|
|
// Draw the entire occupancy map as an image
|
|
rtk_fig_image(
|
|
fig, map->origin_x, map->origin_y, 0,
|
|
map->scale, map->size_x, map->size_y, 16, image, NULL);
|
|
|
|
free(image);
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////
|
|
// Draw a wifi map
|
|
void map_draw_wifi(map_t * map, rtk_fig_t * fig, int index)
|
|
{
|
|
int i, j;
|
|
int level, col;
|
|
map_cell_t * cell;
|
|
uint16_t * image, * mask;
|
|
uint16_t * ipix, * mpix;
|
|
|
|
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
|
|
mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
|
|
|
|
// Draw wifi levels
|
|
for (j = 0; j < map->size_y; j++) {
|
|
for (i = 0; i < map->size_x; i++) {
|
|
cell = map->cells + MAP_INDEX(map, i, j);
|
|
ipix = image + (j * map->size_x + i);
|
|
mpix = mask + (j * map->size_x + i);
|
|
|
|
level = cell->wifi_levels[index];
|
|
|
|
if (cell->occ_state == -1 && level != 0) {
|
|
col = 255 * (100 + level) / 100;
|
|
*ipix = RTK_RGB16(col, col, col);
|
|
*mpix = 1;
|
|
} else {
|
|
*mpix = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Draw the entire occupancy map as an image
|
|
rtk_fig_image(
|
|
fig, map->origin_x, map->origin_y, 0,
|
|
map->scale, map->size_x, map->size_y, 16, image, mask);
|
|
|
|
free(mask);
|
|
free(image);
|
|
}
|
|
|
|
|
|
#endif
|