SLAM auf dem ESP32?

[Holomino]

Weiter geht es mit neuen Features.
Die V0.6 liegt hier (https://c.gmx.net/@31902611639422705...GUZEDN7AmUzQ):
Wenn auf der verlinkten Seite etwas von fehlender Freigabe steht, einmal über F5 im Browser aktualisieren.

Als Update einfach entpacken, mit Arduino öffnen, PWD und SSID aus Common.h anpassen und das Spiffs-Laufwerk aus dem data-Verzeichnis aktualisieren. Ggf. noch die serielle Schnittstelle anpassen (sonst kommen keine Daten).

Die Eroded-map
==============



Der im Fall A) gezeigte Pfad geht zwar nach den Informationen der bisher erstellten Karte über freie Felder, trotzdem würde der dem Pfad folgende Roboter mit dem Hinderniss kollidieren (er ist selber ein Körper).
Entweder berechnet man bei der Pfadsuche also die Abmessungen des Roboters ein, oder, wie in Fall B) gezeigt, man vergrößert die Hindernisse entsprechend: man zeichnet jeden Hindernispunkt der Lidar-map als Klecks mit dem Radius des Roboters in die Eroded-map.
Das funktioniert nach folgenden Spielregeln:
Aus dem Slam-Zyklus heraus werden die Felder der Lidar-map über Grid->Set(x,y,value) letztendlich in Tile->Set(x,y,value) manipuliert.

Verpassen wir Tile->Set einen Rückgabewert, der angibt, in welche Richtung sich der Belegtwert des Feldes (>0 oder <=0) ändert...

int8_t Tile::Set(int x, int y, int8_t val)
{
  int8_t rval =0;
  int offset = y*_width+x;
  if(Data[offset]<=0 && val > 0)
    rval= 5;
  if(Data[offset]>0 && val <= 0)
    rval= -5;

  Data[offset] = val;  
  IsDirty= true;
  IsTested= false;
  return rval;
}

... können wir in Grid->Set mithilfe einer vordefinierten Maske in der Eroded-map den nötigen Tintenklecks einfügen.

//from common.h
// 2 fields additional mask:
// XXX
//XXXXX
//XXOXX
//XXXXX
// XXX
int RobotMask[][2]={        {-1, 2},{0, 2},{1, 2},
                    {-2, 1},{-1, 1},{0, 1},{1, 1},{2, 1},
                    {-2, 0},{-1, 0},{0, 0},{1, 0},{2, 0},
                    {-2, -1},{-1, -1},{0, -1},{1, -1},{2, -1},
                            {-1, -2},{0, -2},{1, -2}
                   };



void Grid::Set(int x, int y, int8_t val)
{
  Tile* tile = GetTile(x,y, true);
  if (tile == nullptr) 
    return;
    
  int8_t e = tile->Set((x-xMin)%SLAM_TILESIZE,(y-yMin)%SLAM_TILESIZE, val);
  if (e !=0)
  {
    int ex, ey;
    for (int i=0;i <sizeof(RobotMask)/8;i++)
    {
      ex= x+RobotMask[i][0];
      ey= y+RobotMask[i][1];
            
      tile = GetTile(ex,ey, true);
      if (tile != nullptr) 
        tile->AddToEroded((ex-xMin)%SLAM_TILESIZE,(ey-yMin)%SLAM_TILESIZE, e);
    }
  }
}

Das Resultat: Dickere Wände, freie Felder sind 0, besetzte Felder haben einen positiven Wert.
Negative Werte werden nicht eingenommen.

Wissenswert vielleicht noch: Die Umstellung der Ansicht Lidar-map/ Eroded-map erfolgt über
#define DRAWING_SHOW_ERODED
(wie immer in Common.h)

Der Pathfinder
==============
In der jetzigen Testimplementierung sucht der Pathfinder den Weg von der aktuellen Roboterposition zum Zielpunkt (0;0). Den Zielpunkt kann man im Frontend durch Klicken auf die Map umsetzen. In der Debug-Sektion gibt der "Pathfinder-Report" weitere Infos.

Zum Algorithmus: Nachdem wir mit der Eroded-map quasi das Speicheraufkommen für die lapidare Aufgabe der Kollisionsvermeidung um Hindernisse herum verdoppelt haben, wurde es Zeit, zumindest beim Pathfinder nach einer der Hardware angemessenen Lösung zu suchen.



Das Ergebnis im Bild: Links der aktuell implementierte Algorithmus, im Vergleich rechts ein A* auf dem PC. Das sieht auf den ersten Blick nicht optimal aus.
Auf den zweiten Blick allerdings zeigt auch der A* keinen perfekten Weg. Knoten sind nun mal auf einem Array Nachbarfelder. Die können nur horizontal, vertikal oder diagonal sein. Entsprechend ermittelt auch der A* nicht die über die blaue Linie angedeutete Abkürzung.

Beim A* bietet es sich an, während der Fahrt über den Pfad (Pathfollower) diese Abkürzungen zu suchen, indem man mithilfe des bereits im Slam verwendeten Bresenham die vor sich liegenden Felder abtastet. Wenn man dadurch ein´weiter entferntes Feld hinter der nöchsten Kurve geradlinig erreichen kann, dann kann man die Kurve also schneiden.

Und ja, ich bin der Meinung, das kann man auch mit dem Ergebnis des ESP-Patfinders. Mit ein bisschen Spielen wird also während der Fahrt nicht viel Anderes bei rumkommen.

Die Handbremse lösen
===================
Wer noch einmal die alte Version 0.5 hervorholt und hier in der loop() in ESP32Slam.ino ein vTaskDelay(100) einfügt, wird bemerken, dass sich die Zykluszeiten des Slam auf dem ESP32-S2 um ca. 40% verbessern.
Ich denke mal, dass auch die loop() eine RTOS-Task mit Priority>0 ist und der leere Rumpf alleine die anderen Tasks gewaltig ausbremst. Anders kann ich es mir nicht erklären.