Add Bayesian Obstacle Grid Mapping with TFmini Lidar Support and Simulation (#3510)

sw/airborne/firmwares/rover/obstacles/rover_obstacles.c

@@ -0,0 +1,412 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file rover_obstacles.c
+ *  @brief functions to create a grid map
+ *
+ */
+
+
+// Depends of tfmini lidar(could be replaced by other lidar)
+
+#include "rover_obstacles.h"
+#include "math/pprz_geodetic_float.h"
+
+#include "modules/datalink/telemetry.h"
+#include "state.h"
+
+#include "firmwares/rover/navigation.h"
+
+
+// Probability Map
+#define P_FREE    0.4   // cell observed as free (negative log-odds)
+#define P_OCC     0.7   // cell observed as occupied (positive log-odds)
+#define L_MIN    -127   // minimum saturation
+#define L_MAX     127   // maximum saturation
+#define L0         0    // initial value (unknown)
+#define P_T       0.95   // Threshold to consider a cell occupied/free
+
+#define SCALE    20.0f   // scaling of log-odds float to int8_t --> With 20, max prob 0.995
+
+
+// Abi call
+#ifndef OBSTACLES_RECEIVE_ID
+#define OBSTACLES_RECEIVE_ID ABI_BROADCAST
+#endif
+
+
+#define DECAY_INTERVAL 5000 // ms for obstacle probability to decay
+#define DECAY 5 // Decay per second
+static uint32_t last_s = 0;
+
+
+#include "modules/core/abi.h"
+static abi_event lidar_ev;
+static void lidar_cb(uint8_t sender_id, uint32_t stamp, float distance, float angle);
+float max_distance = 5;
+
+
+// Global variables (to avoid recalculating log all the time)
+static float POCC = 0;
+static float PFREE = 0;
+static float PT = 0;
+
+static int8_t LT, LOCC, LFREE;
+
+world_grid obstacle_grid;
+uint8_t grid_block_size = GRID_BLOCK_SIZE; // This is only used in CBF
+
+
+#if PERIODIC_TELEMETRY
+static void send_obstacle_grid(struct transport_tx *trans, struct link_device *dev)
+{
+  // Send all cols from obstacle_grid.now_row in a cyclic pattern
+  pprz_msg_send_OBSTACLE_GRID(trans, dev, AC_ID,
+                              &obstacle_grid.dx,
+                              &obstacle_grid.dy,
+                              &obstacle_grid.xmin,
+                              &obstacle_grid.xmax,
+                              &obstacle_grid.ymin,
+                              &obstacle_grid.ymax,
+                              &obstacle_grid.now_row,
+                              N_COL_GRID, obstacle_grid.world[obstacle_grid.now_row]);
+
+  obstacle_grid.now_row = (obstacle_grid.now_row + 1) % N_ROW_GRID;
+}
+static void send_grid_init(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_GRID_INIT(trans, dev, AC_ID,
+                          &obstacle_grid.dx,
+                          &obstacle_grid.dy,
+                          &obstacle_grid.xmin,
+                          &obstacle_grid.xmax,
+                          &obstacle_grid.ymin,
+                          &obstacle_grid.ymax,
+                          &obstacle_grid.map.LT
+                         );
+}
+#endif
+
+void init_grid(uint8_t pa, uint8_t pb)
+{
+
+  int i, j;
+  for (i = 0; i < N_ROW_GRID; i++) {
+    for (j = 0; j < N_COL_GRID; j++) {
+      obstacle_grid.world[i][j] = L0;
+    }
+  }
+  // Rows in X, cols in Y
+  obstacle_grid.xmin = WaypointX(pa);
+  obstacle_grid.xmax = WaypointX(pb);
+  obstacle_grid.ymin = WaypointY(pa);
+  obstacle_grid.ymax = WaypointY(pb);
+  obstacle_grid.dx   = (obstacle_grid.xmax - obstacle_grid.xmin) / ((float)N_COL_GRID);
+  obstacle_grid.dy   = (obstacle_grid.ymax - obstacle_grid.ymin) / ((float)N_ROW_GRID);
+  obstacle_grid.now_row = 0;
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GRID_INIT, send_grid_init);
+#endif
+  obstacle_grid.is_ready = 1;
+
+}
+
+// Same as init_grid but with 4 waypoints (you can give the 4 wp in any order,
+// but you need to have them in the correct order in the flightplan for painting the border in the GCS).
+void init_grid_4(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4)
+{
+  float xmin = fminf(fminf(WaypointX(wp1), WaypointX(wp2)), fminf(WaypointX(wp3), WaypointX(wp4)));
+  float xmax = fmaxf(fmaxf(WaypointX(wp1), WaypointX(wp2)), fmaxf(WaypointX(wp3), WaypointX(wp4)));
+  float ymin = fminf(fminf(WaypointY(wp1), WaypointY(wp2)), fminf(WaypointY(wp3), WaypointY(wp4)));
+  float ymax = fmaxf(fmaxf(WaypointY(wp1), WaypointY(wp2)), fmaxf(WaypointY(wp3), WaypointY(wp4)));
+
+  obstacle_grid.xmin = xmin;
+  obstacle_grid.xmax = xmax;
+  obstacle_grid.ymin = ymin;
+  obstacle_grid.ymax = ymax;
+
+  obstacle_grid.dx = (xmax - xmin) / ((float)N_COL_GRID);
+  obstacle_grid.dy = (ymax - ymin) / ((float)N_ROW_GRID);
+
+  obstacle_grid.now_row = 0;
+  obstacle_grid.is_ready = 1;
+
+  obstacle_grid.map.threshold = (float) P_T;
+  obstacle_grid.map.occ = (float) P_OCC;
+  obstacle_grid.map.free = (float) P_FREE;
+  obstacle_grid.map.decay = (uint8_t) DECAY;
+
+  memset(obstacle_grid.world, 0, sizeof(obstacle_grid.world));
+
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GRID_INIT, send_grid_init);
+#endif
+
+  AbiBindMsgOBSTACLE_DETECTION(OBSTACLES_RECEIVE_ID, &lidar_ev, lidar_cb);
+
+  // Send the message to the GCS
+  DOWNLINK_SEND_GRID_INIT(
+    DefaultChannel,
+    DefaultDevice,
+    &obstacle_grid.dx,
+    &obstacle_grid.dy,
+    &obstacle_grid.xmin,
+    &obstacle_grid.xmax,
+    &obstacle_grid.ymin,
+    &obstacle_grid.ymax,
+    &obstacle_grid.map.LT
+  );
+}
+
+
+void obtain_cell_xy(float px, float py, int *cell_x, int *cell_y)
+{
+
+  // Must be >= 0, xmin <= px <= xmax
+  //               ymin <= py <= ymax
+  *cell_x = (int)((px - obstacle_grid.xmin) / obstacle_grid.dx); // Like floor
+  *cell_y = (int)((py - obstacle_grid.ymin) / obstacle_grid.dy);
+
+  *cell_x = (*cell_x >= 0) ? *cell_x : 0;
+  *cell_y = (*cell_y >= 0) ? *cell_y : 0;
+}
+
+void fill_cell(float px, float py)
+{
+
+  int cx, cy;
+  obtain_cell_xy(px, py, &cx, &cy);
+  obstacle_grid.world[cy][cx] = 1; // Row, col
+
+}
+
+void fill_bayesian_cell(float px, float py)
+{
+
+  if (!obstacle_grid.is_ready) { return; }
+
+  int cx, cy;
+  obtain_cell_xy(px, py, &cx, &cy);
+
+  // Gets the cell where the rover is
+  int rx, ry;
+  struct EnuCoor_f rover_pos;
+  rover_pos = *stateGetPositionEnu_f();
+  obtain_cell_xy(rover_pos.x, rover_pos.y, &rx, &ry);
+
+  update_line_bayes(rx, ry, cx, cy);   // Free between rover and obstacle
+  compute_cell_bayes(cx, cy, true);     // Ocupied at the last point
+}
+
+// Fills the free cells when there is no lidar measurement
+void fill_free_cells(float lidar, float angle)
+{
+  if (!obstacle_grid.is_ready) { return; }
+
+  // Gets the cell where the rover is
+  int rx, ry;
+  struct EnuCoor_f rover_pos;
+  rover_pos = *stateGetPositionEnu_f();
+  obtain_cell_xy(rover_pos.x, rover_pos.y, &rx, &ry);
+  if (rx < 0 || rx >= N_COL_GRID || ry < 0 || ry >= N_ROW_GRID) {
+    return;
+  }
+
+  // If there is no lidar measurement, mark cells as free
+  if (lidar == 0.0f) {
+    // Calculates the fictitious obstacle
+    int cx, cy;
+    float theta = stateGetNedToBodyEulers_f()->psi;
+    float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
+
+    float px = rover_pos.x + (max_distance * cosf(corrected_angle));
+    float py = rover_pos.y + (max_distance * sinf(corrected_angle));
+
+    obtain_cell_xy(px, py, &cx, &cy);
+
+    update_line_bayes(rx, ry, cx, cy);   // Free between rover and obstacle
+    return;
+  } else {
+    compute_cell_bayes(rx, ry, false);    // Free at the end point
+  }
+}
+
+
+// Function for the map to forget the obstacles
+void decay_map()
+{
+  uint32_t now_s = get_sys_time_msec();
+
+  if (now_s > (last_s + DECAY_INTERVAL)) {
+    last_s = now_s;
+    for (int y = 0; y < N_ROW_GRID; y++) {
+      for (int x = 0; x < N_COL_GRID; x++) {
+        int8_t *cell = &obstacle_grid.world[y][x];
+        if (*cell == 0) {
+          continue;
+        } else if (*cell > 0) {
+          int updated = (*cell > obstacle_grid.map.decay) ? *cell - obstacle_grid.map.decay : 0;
+          update_cell(x, y, updated);
+        } else if (*cell < 0) {
+          int updated = (*cell < - obstacle_grid.map.decay) ? *cell + obstacle_grid.map.decay : 0;
+          update_cell(x, y, updated);
+        }
+      }
+    }
+  }
+}
+
+
+
+/*******************************************************************************
+ *                                                                             *
+ *  Aux functions                                                           *
+ *                                                                             *
+ ******************************************************************************/
+
+
+// Bresenham algorithm
+void update_line_bayes(int x0, int y0, int x1, int y1)
+{
+  int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
+  int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
+  int err = dx + dy;
+
+  while (1) {
+    if (x0 == x1 && y0 == y1) { break; }
+    compute_cell_bayes(x0, y0, false); // Libre
+    int e2 = 2 * err;
+    if (e2 >= dy) { err += dy; x0 += sx; }
+    if (e2 <= dx) { err += dx; y0 += sy; }
+  }
+}
+
+
+// Decide to send the cell (0 unknown, 1 occupied, 2 free)
+void update_cell(int x, int y, int new_value)
+{
+
+  int8_t *cell = &obstacle_grid.world[y][x];
+  int8_t old_value = *cell;
+
+  uint8_t old_state = (old_value > LT) ? 1 : (old_value < -LT) ? 2 : 0;
+  uint8_t new_state = (new_value > LT) ? 1 : (new_value < -LT) ? 2 : 0;
+  obstacle_grid.map.LT = LT; // For the GCS
+
+  *cell = (int8_t)new_value;
+
+  if (old_state != new_state) {
+    DOWNLINK_SEND_GRID_CHANGES(DefaultChannel, DefaultDevice, &y, &x, &new_value);
+    // printf("Cell (%d, %d) updated from %d to %d (delta: %d)\n", x, y, old_value, *cell, delta);
+  }
+}
+
+
+void compute_cell_bayes(int x, int y, bool is_occupied)
+{
+  if (x < 0 || x >= N_COL_GRID || y < 0 || y >= N_ROW_GRID) {
+    return;
+  }
+  int8_t *cell = &obstacle_grid.world[y][x];
+
+  check_probs();
+
+  int delta = is_occupied ? LOCC : LFREE;
+  int updated = *cell + delta;
+  if (updated > L_MAX) { updated = L_MAX; }
+  if (updated < L_MIN) { updated = L_MIN; }
+
+  update_cell(x, y, updated);
+}
+
+
+//int8_t *LOCC, int8_t *LFREE, int8_t *LT
+void check_probs(void)
+{
+
+  if ((obstacle_grid.map.occ != POCC) || (obstacle_grid.map.free != PFREE)) {
+    LOCC = (int8_t)(SCALE * logf(obstacle_grid.map.occ / (1.0f - obstacle_grid.map.occ)));
+    LFREE = (int8_t)(SCALE * logf(obstacle_grid.map.free / (1.0f - obstacle_grid.map.free)));
+    POCC = obstacle_grid.map.occ;
+    PFREE = obstacle_grid.map.free;
+    // printf("LOCC: %d, LFREE: %d\n", *LOCC, *LFREE);
+  }
+
+  if (obstacle_grid.map.threshold != PT) {
+    LT = (int8_t)(SCALE * logf(obstacle_grid.map.threshold / (1.0f - obstacle_grid.map.threshold)));
+    PT = obstacle_grid.map.threshold;
+  }
+}
+
+
+
+/*******************************************************************************
+ *                                                                             *
+ *  Testing functions                                                           *
+ *                                                                             *
+ ******************************************************************************/
+
+
+void ins_update_lidar(float distance, float angle)
+{
+
+  if (!stateIsLocalCoordinateValid()) {
+    return;
+  }
+
+  if (!wall_system.converted_to_ltp) {
+    convert_walls_to_ltp();
+  }
+
+  // In case there is real measured for the Lidar
+  if (distance == 0.0) {
+    return;
+  }
+
+  // Everything is in ENU
+  float x_rover = stateGetPositionEnu_f()->x;
+  float y_rover = stateGetPositionEnu_f()->y;
+
+
+  float theta = stateGetNedToBodyEulers_f()->psi;
+  float corrected_angle = M_PI / 2 - angle * M_PI / 180 - theta;
+
+  struct FloatVect2 obstacle = {0.0, 0.0};
+  obstacle.x = x_rover + (distance * cosf(corrected_angle));
+  obstacle.y = y_rover + (distance * sinf(corrected_angle));
+
+  // Fill the grid
+#ifdef USE_GRID
+  fill_bayesian_cell(obstacle.x, obstacle.y);
+#endif
+
+}
+
+
+
+static void lidar_cb(uint8_t __attribute__((unused)) sender_id,
+                     uint32_t stamp __attribute__((unused)),
+                     float distance, float angle)
+{
+  ins_update_lidar(distance, angle);
+}

sw/airborne/firmwares/rover/obstacles/rover_obstacles.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2025 Alejandro Rochas <alrochas@ucm.es>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file rover_obstacles.hs
+ *  @brief functions to create a grid map
+ *
+ */
+
+
+#ifndef ROVER_OBSTACLES_H
+#define ROVER_OBSTACLES_H
+
+
+#ifndef N_COL_GRID
+#define N_COL_GRID 100
+#endif
+
+#ifndef N_ROW_GRID
+#define N_ROW_GRID 100
+#endif
+
+#include "std.h"
+
+
+typedef struct{
+	int8_t LT;					// Threshold for occupied/cells (log-odds)
+	float threshold;		// Threshold for occupied/free cells
+	float occ;					// Occupied cells probability
+	float free;					// Free cells probability
+	uint8_t decay;				// Decay Probability (log-odds)
+} bayesian_map;
+
+
+typedef struct{
+	int8_t world[N_ROW_GRID][N_COL_GRID];
+	float xmin;
+	float xmax;
+	float ymin;
+	float ymax;
+	float home[2];
+	float dx;
+	float dy;
+	uint16_t now_row;
+	int is_ready;
+	bayesian_map map;
+} world_grid;
+
+extern world_grid obstacle_grid;
+
+#define GRID_BLOCK_SIZE 4
+extern uint8_t grid_block_size; // This is only used in CBF
+
+
+extern void init_grid(uint8_t pa, uint8_t pb);
+extern void init_grid_4(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4);
+extern void obtain_cell_xy(float px, float py, int *cell_x, int *cell_y);
+
+extern void fill_cell(float px, float py);
+extern void fill_bayesian_cell(float px, float py);
+extern void fill_free_cells(float lidar, float angle);
+
+extern void decay_map(void);
+extern void update_line_bayes(int x0, int y0, int x1, int y1);
+extern void update_cell(int x, int y, int new_value);
+extern void compute_cell_bayes(int x, int y, bool is_occupied);
+extern void check_probs(void);
+
+
+#endif // ROVER_OBSTACLES_H