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,118 @@
+/*
+ *  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: Range routines
+ * Author: Andrew Howard
+ * Date: 18 Jan 2003
+ * CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
+**************************************************************************/
+
+#include <assert.h>
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include "nav2_amcl/map/map.hpp"
+
+// Extract a single range reading from the map.  Unknown cells and/or
+// out-of-bound cells are treated as occupied, which makes it easy to
+// use Stage bitmap files.
+double map_calc_range(map_t * map, double ox, double oy, double oa, double max_range)
+{
+  // Bresenham raytracing
+  int x0, x1, y0, y1;
+  int x, y;
+  int xstep, ystep;
+  char steep;
+  int tmp;
+  int deltax, deltay, error, deltaerr;
+
+  x0 = MAP_GXWX(map, ox);
+  y0 = MAP_GYWY(map, oy);
+
+  x1 = MAP_GXWX(map, ox + max_range * cos(oa));
+  y1 = MAP_GYWY(map, oy + max_range * sin(oa));
+
+  if (abs(y1 - y0) > abs(x1 - x0)) {
+    steep = 1;
+  } else {
+    steep = 0;
+  }
+
+  if (steep) {
+    tmp = x0;
+    x0 = y0;
+    y0 = tmp;
+
+    tmp = x1;
+    x1 = y1;
+    y1 = tmp;
+  }
+
+  deltax = abs(x1 - x0);
+  deltay = abs(y1 - y0);
+  error = 0;
+  deltaerr = deltay;
+
+  x = x0;
+  y = y0;
+
+  if (x0 < x1) {
+    xstep = 1;
+  } else {
+    xstep = -1;
+  }
+  if (y0 < y1) {
+    ystep = 1;
+  } else {
+    ystep = -1;
+  }
+
+  if (steep) {
+    if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1) {
+      return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
+    }
+  } else {
+    if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1) {
+      return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
+    }
+  }
+
+  while (x != (x1 + xstep * 1)) {
+    x += xstep;
+    error += deltaerr;
+    if (2 * error >= deltax) {
+      y += ystep;
+      error -= deltax;
+    }
+
+    if (steep) {
+      if (!MAP_VALID(map, y, x) || map->cells[MAP_INDEX(map, y, x)].occ_state > -1) {
+        return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
+      }
+    } else {
+      if (!MAP_VALID(map, x, y) || map->cells[MAP_INDEX(map, x, y)].occ_state > -1) {
+        return sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) * map->scale;
+      }
+    }
+  }
+  return max_range;
+}