add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,147 @@
+/*
+ *  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