Ngoren/occupancy gridcells fix

[nivgo]

Occupancy Grid — How It Works

1. Overall Pipeline

flowchart TD
    FW["🔲 D585S Firmware\npacked bitmask — 1 bit per cell\n(1=occupied, 0=not occupied)"]

    subgraph fn["publishOccupancyFrame()"]
        INIT["Init every cell → -1\n(unknown)"]
        FOV{"FOV cone check\n|y| ≥ x · tan(hfov/2) ?"}
        RANGE{"Max range check\nx > occupancy_max_range ?"}
        BIN["Assign to angular bin\nθ = atan2(y, x)"]
        RAY["Scan ray nearest → farthest\nwith per-bin occlusion state"]
        OCC{"Occupied\nbit set?"}
        SHAD{"Ray already\noccluded?"}
        SPREAD["Spread shadow to\n± n_spread neighbour bins\nn_spread = ceil(cell_size·N_bins / 2·x·bin_range)"]
    end

    FREE["0\nconfirmed free"]
    OB["100\noccupied"]
    UNK["-1\nunknown"]
    OUT["📦 nav_msgs/OccupancyGrid"]

    FW --> INIT --> FOV
    FOV -->|outside camera FOV| UNK
    FOV -->|inside FOV| RANGE
    RANGE -->|beyond 2.5 m default| UNK
    RANGE -->|within range| BIN --> RAY --> OCC
    OCC -->|yes| OB --> SPREAD
    OCC -->|no| SHAD
    SHAD -->|yes| UNK
    SHAD -->|no| FREE
    SPREAD -->|"neighbours → -1"| UNK
    FREE --> OUT
    OB  --> OUT
    UNK --> OUT

Loading

---

2. Cell Value Semantics

flowchart LR
    subgraph legend["Cell value in OccupancyGrid.data[]"]
        direction TB
        A["0\n✅ confirmed free\nRay passed through — no obstacle seen"]
        B["100\n🔴 occupied\nFirmware bit = 1"]
        C["-1\n❓ unknown\nOutside FOV, beyond max range,\nor shadowed behind an obstacle"]
    end

Loading

---

3. Live Output

Real grid published by the driver, shown in rviz with the live point cloud overlaid.
White = free (0), gray = unknown (-1), occupied cells (100) sit under the detected boxes —
note the FOV wedge, the shadows fanning out behind each obstacle, and the unknown
region beyond occupancy_max_range:

---

4. Column-Based vs True Angular Raycasting

OLD — Column-based (wrong)          NEW — Angular raycasting (correct)
─────────────────────────────       ─────────────────────────────────────
Camera at bottom (x=0)             Camera at bottom (x=0)

      Y=0.5m fixed                        angle θ fixed
         │                                    ╱
x=0.5m  ●                          x=0.5m  ●  (y=0.5m)
         │                                    ╲
x=1.0m  ●                          x=1.0m    ●  (y=1.0m)
         │                                      ╲
x=2.0m  ●                          x=2.0m        ●  (y=2.0m)
         │
x=3.0m  ●                          Shadow FANS outward
                                    y grows with depth: y = x · tan(θ)
Shadow is a vertical stripe         ✔ physically correct
✘ wrong at wide angles

---

5. Shadow Propagation Along a Ray

Ray (single angular bin), scanning nearest → farthest:

  Camera
    │
    ▼
[ free  ]  ← no obstacle yet, clear line of sight  → cell = 0
[ free  ]
[ free  ]
[BLOCKED]  ← firmware bit = 1                       → cell = 100  ← occlusion starts here
[shadow ]  ← ray is occluded                        → cell = -1
[shadow ]
[shadow ]  ← stays unknown all the way to max range → cell = -1

---

6. Gap-Ray Fix (Shadow Spreading)

Without the fix, a ray can slip through the gap between two adjacent occupied cells
that fall in different angular bins:

BEFORE gap-ray fix                    AFTER gap-ray fix
                                      (n_spread bins on each side)

Angular bins:  …23  24  25  26…       Angular bins:  …23  24  25  26…
                    │   │                                  │   │
depth=near          ■   ■  ← 2 occupied cells             ■   ■
depth=mid       0   0   0   0    ← gap ray free!      -1  -1  -1  -1  ← all shadowed
depth=far       0   0   0   0                         -1  -1  -1  -1

■ = occupied   0 = free   -1 = unknown/shadow

n_spread = ceil( cell_size × N_bins / (2 × x_obstacle × bin_range) )
         = large at close range (wide angular footprint)
         = 1    at far range   (narrow angular footprint)

---

7. FOV Masking

Top-down view of the grid (camera at bottom centre):

        ╔═══════════════════════╗   ← far edge (x = max_range)
        ║  -1  -1 │  0   0 │-1 -1║  ← -1 outside FOV cone
        ║  -1  -1 │  0   0 │-1 -1║
        ║  -1   0 │  0   0 │ 0 -1║
        ║   0   0 │100 100 │ 0  0║  ← obstacles
        ║   0   0 │ -1  -1 │ 0  0║  ← shadow behind obstacles
        ║   0   0 │ -1  -1 │ 0  0║
        ╚═══════════════════════╝   ← near edge (x ≈ 0)
                   camera

  FOV cone boundary: |y| = x · tan(hfov/2)
  Anything outside the cone → -1 (unknown)
  Anything beyond max_range → -1 (unknown)

---

8. Coordinate Mapping: Firmware → OccupancyGrid

Firmware layout                    OccupancyGrid layout (ROS convention)
(fw_row=0 = farthest)              (col_idx=0 = nearest, X forward)

fw_row=0   [col 0..cols-1]    →   og_col_idx = width-1-fw_row  (maps far→high X)
fw_row=1   [col 0..cols-1]         og_row_idx = cols-1-fw_col  (maps left→high Y)
...
fw_row=rows-1 [col 0..cols-1] →   data[og_row_idx * width + og_col_idx]

Firmware "cols" = lateral (Y axis) = msg.info.height
Firmware "rows" = depth   (X axis) = msg.info.width

[sysrsbuild-gh]

Can one of the admins verify this patch?

[Nir-Az]

@remibettan please help reviewing this PR, I also suggest running it