App/www/map-occupancy-canvas.js

(() => {
  /**
   * RViz-style occupancy grid renderer for map view (Canvas 2D).
   *
   * ROS map_server / nav_msgs/OccupancyGrid:
   *   0   = free
   *   100 = occupied
   *  -1   = unknown
   *
   * Grid data is row-major with index 0 at the bottom-left cell (world +Y up).
   * Canvas pixels use top-left origin — Y is flipped when painting.
   */

  const DEFAULT_PALETTE = {
    free: [254, 254, 254, 255],
    occupied: [0, 0, 0, 255],
    unknown: [180, 180, 180, 255],
  };

  const DEFAULT_THRESHOLDS = {
    occupied_thresh: 0.65,
    free_thresh: 0.196,
    negate: 0,
  };

  function metaThresholds(meta) {
    return {
      occupied_thresh: Number(meta?.occupied_thresh ?? DEFAULT_THRESHOLDS.occupied_thresh),
      free_thresh: Number(meta?.free_thresh ?? DEFAULT_THRESHOLDS.free_thresh),
      negate: Number(meta?.negate ?? DEFAULT_THRESHOLDS.negate),
    };
  }

  /** Trinary mode (Nav2/map_server): darkness or lightness vs thresholds in [0, 1]. */
  function grayToOccValue(gray, meta) {
    const { occupied_thresh, free_thresh, negate } = metaThresholds(meta);
    const lightness = gray / 255;
    const probability = negate ? lightness : 1 - lightness;
    if (probability > occupied_thresh) return 100;
    if (probability < free_thresh) return 0;
    return -1;
  }

  function occToRgba(value, palette) {
    if (value < 0) return palette.unknown;
    if (value >= 100) return palette.occupied;
    if (value === 0) return palette.free;
    return value > 50 ? palette.occupied : palette.free;
  }

  function paletteFrom(opts) {
    const p = opts?.palette || {};
    return {
      free: p.free || DEFAULT_PALETTE.free,
      occupied: p.occupied || DEFAULT_PALETTE.occupied,
      unknown: p.unknown || DEFAULT_PALETTE.unknown,
    };
  }

  function ensureCanvasSize(canvas, width, height) {
    if (canvas.width !== width || canvas.height !== height) {
      canvas.width = width;
      canvas.height = height;
    }
  }

  function decodeGridData(data) {
    if (data instanceof Int8Array || data instanceof Uint8Array) return data;
    if (Array.isArray(data)) return data;
    if (typeof data === "string" && data.length) {
      try {
        const binary = atob(data);
        const out = new Int8Array(binary.length);
        for (let i = 0; i < binary.length; i++) out[i] = binary.charCodeAt(i);
        return out;
      } catch {
        return [];
      }
    }
    return [];
  }

  /**
   * Paint ROS occupancy grid onto canvas.
   * @param {HTMLCanvasElement} canvas
   * @param {{ width: number, height: number, data: number[]|Int8Array|string }} grid
   * @param {{ palette?: object }} [opts]
   */
  function renderGrid(canvas, grid, opts = {}) {
    if (!canvas || !grid) return false;
    const width = Number(grid.width) | 0;
    const height = Number(grid.height) | 0;
    if (!width || !height) return false;

    const data = decodeGridData(grid.data);
    if (!data.length) return false;

    ensureCanvasSize(canvas, width, height);
    const ctx = canvas.getContext("2d");
    if (!ctx) return false;

    const palette = paletteFrom(opts);
    const imageData = ctx.createImageData(width, height);
    const px = imageData.data;

    for (let row = 0; row < height; row++) {
      const srcRow = height - 1 - row;
      for (let col = 0; col < width; col++) {
        const idx = srcRow * width + col;
        const value = idx < data.length ? data[idx] : -1;
        const rgba = occToRgba(value, palette);
        const dst = (row * width + col) * 4;
        px[dst] = rgba[0];
        px[dst + 1] = rgba[1];
        px[dst + 2] = rgba[2];
        px[dst + 3] = rgba[3];
      }
    }

    ctx.putImageData(imageData, 0, 0);
    return true;
  }

  /**
   * Convert map_server grayscale image to RViz-style occupancy colors.
   * @param {HTMLCanvasElement} canvas
   * @param {CanvasImageSource} source
   * @param {object} [meta] — negate, occupied_thresh, free_thresh
   * @param {{ palette?: object }} [opts]
   */
  function renderFromImage(canvas, source, meta = {}, opts = {}) {
    if (!canvas || !source) return false;
    const width = source.naturalWidth || source.videoWidth || source.width;
    const height = source.naturalHeight || source.videoHeight || source.height;
    if (!width || !height) return false;

    ensureCanvasSize(canvas, width, height);
    const ctx = canvas.getContext("2d");
    if (!ctx) return false;

    const scratch = document.createElement("canvas");
    scratch.width = width;
    scratch.height = height;
    const sctx = scratch.getContext("2d");
    if (!sctx) return false;

    sctx.drawImage(source, 0, 0, width, height);
    const sampled = sctx.getImageData(0, 0, width, height);
    const src = sampled.data;
    const palette = paletteFrom(opts);
    const imageData = ctx.createImageData(width, height);
    const dst = imageData.data;
    const thresholds = metaThresholds(meta);

    for (let i = 0, p = 0; p < width * height; p++, i += 4) {
      const gray = src[i];
      const occ = grayToOccValue(gray, thresholds);
      const rgba = occToRgba(occ, palette);
      dst[i] = rgba[0];
      dst[i + 1] = rgba[1];
      dst[i + 2] = rgba[2];
      dst[i + 3] = 255;
    }

    ctx.putImageData(imageData, 0, 0);
    return true;
  }

  window.MapOccupancyCanvas = {
    DEFAULT_PALETTE,
    DEFAULT_THRESHOLDS,
    grayToOccValue,
    occToRgba,
    decodeGridData,
    renderGrid,
    renderFromImage,
  };
})();