feat: Depth perception from mono camera

CLAUDE.md

@@ -101,9 +101,11 @@ Workstation-only Gazebo simulation, kept separate from `mote_bringup` so it can
 - `test/sim_smoke/` — `run_sim_smoke.sh` + `verify_sim.py`, the `pixi run sim-test` gate.
 
 ### `mote_perception` (Python/ament)
-Home for camera-derived perception nodes — **L0 (Foundation)** of the vision pipeline. Runs on the robot (it will eventually feed Nav2), so unlike `mote_simulation` it is synced to the Pi. Contains:
-- `mote_perception/camera_monitor.py` — a dependency-light camera health monitor (rclpy + sensor_msgs only, no OpenCV). Subscribes to `image` and logs measured frame rate, resolution, and encoding on a timer, warning on dropouts. Registered as the `camera_monitor` console_script; it is the template later perception nodes follow.
-- `launch/perception_launch.py` — declares `use_sim_time` (applied via `SetParameter`) and starts `camera_monitor` with `image` remapped to `/image_raw`. Marked as the extension point where L1 nodes (rectify, depth, detection) attach.
+Home for camera-derived perception. Runs on the robot (feeds Nav2), so unlike `mote_simulation` it is synced to the Pi. Contains:
+- `mote_perception/camera_monitor.py` — a dependency-light camera health monitor (rclpy + sensor_msgs only, no OpenCV). Subscribes to `image` and logs measured frame rate, resolution, and encoding on a timer, warning on dropouts. Registered as the `camera_monitor` console_script.
+- **L1 depth-obstacle pipeline** — turns the mono camera into `/camera_obstacles` (PointCloud2) for Nav2's `camera_layer`. Split across: `depth_obstacle_node.py` (torch-free rclpy node: compressed image → server → rescale → back-project → level → z/range gates), `depth_wire.py` (the socket protocol spec + `DepthClient`, shared by node/server/tools), `lidar_rescale.py` (per-frame Theil-Sen affine-in-disparity metric rescale anchored to lidar), `ground_projection.py` (camera↔base geometry: `GroundProjector`, floor-plane fit, leveling). Inference runs off-board: `tools/depth_server.py` in the `depth` pixi env (torch, no ROS); `pixi run depth` starts server + node on a workstation sharing the robot's DDS graph.
+- `tools/` — offline bag harnesses (`depth_bag_replay`, `depth_bag_eval`, `depth_obstacles`, `bag_overlay`, shared `bag_utils`) and the live `measure_camera_pitch`; see `mote_perception/README.md` for the inventory.
+- `launch/perception_launch.py` — declares `use_sim_time` (applied via `SetParameter`) and starts `camera_monitor` with `image` remapped to `/image_raw`.
 - `config/` — camera-calibration home. `camera_info.default.yaml` is a committed fallback calibration for the UGREEN webcam; a per-robot `~/.mote/camera_calibration.yaml` (outside the repo) overrides it. `mote_launch.py` prefers the `~/.mote` file when present, else `robot.yaml`'s `camera.default_info_url`, passing the result to `v4l2_camera_node` as `camera_info_url`. `config/README.md` documents when/how to calibrate (with the printable checkerboard).
 - Compressed transport is already provided by the `image-transport-plugins` dep, so the camera publishes `/image_raw/compressed`; off-board/RViz consumers should prefer it. See `mote_perception/README.md`.
 

mote_bringup/config/mote.rviz

@@ -6,13 +6,14 @@ Panels:
       Expanded:
         - /Base1
         - /Sensors1
+        - /Sensors1/Camera Obstacles1/Autocompute Value Bounds1
         - /SLAM1
         - /Navigation1/Raw Plan1
         - /Navigation1/Global Plan1
         - /Navigation1/DWB Trajectories1
         - /Navigation1/DWB Local Plan1
       Splitter Ratio: 0.5
-    Tree Height: 238
+    Tree Height: 221
   - Class: rviz_common/Views
     Expanded:
       - /Current View1
@@ -186,7 +187,7 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 49
+          Max Intensity: 46
           Min Color: 0; 0; 0
           Min Intensity: 2
           Name: LaserScan
@@ -205,6 +206,72 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 1.600000023841858
+            Min Value: 0
+            Value: false
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Camera Obstacles
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 2
+          Size (m): 0.009999999776482582
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /camera_obstacles
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 1.600000023841858
+            Min Value: 0
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: false
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: Camera Cloud (full)
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.019999999552965164
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /camera_cloud_full
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: false
       Enabled: true
       Name: Sensors
     - Class: rviz_common/Group
@@ -236,8 +303,7 @@ Visualization Manager:
           Enabled: true
           Name: Pose Graph
           Namespaces:
-            slam_toolbox: true
-            slam_toolbox_edges: true
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -515,7 +581,21 @@ Visualization Manager:
             Reliability Policy: Best Effort
             Value: /image_raw
           Value: false
-      Enabled: false
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Depth
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /camera_depth
+          Value: true
+      Enabled: true
       Name: Debug
   Enabled: true
   Global Options:
@@ -556,39 +636,46 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: -0.1250002235174179
-      Class: rviz_default_plugins/TopDownOrtho
+      Class: rviz_default_plugins/Orbit
+      Distance: 5.77841854095459
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 205.4888916015625
+      Pitch: 1.1697959899902344
       Target Frame: base_link
-      Value: TopDownOrtho (rviz_default_plugins)
-      X: 0
-      Y: 0
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 3.542396068572998
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: false
+  Depth:
+    collapsed: false
   Displays:
     collapsed: false
-  Height: 964
+  Height: 1308
   Hide Left Dock: false
   Hide Right Dock: false
   Navigation:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000002000000000000015d0000036afc0200000004fb000000100044006900730070006c006100790073010000003d0000012b000000c900fffffffb0000000a00560069006500770073010000016e000000f5000000a400fffffffb0000000c00430061006d006500720061020000041f000000c8000001c3000000d6fb0000000c00540065006c0065006f007001000002690000013e0000013e00ffffff00000001000001ce0000036afc0200000003fb00000014004e0061007600690067006100740069006f006e010000003d000001df000001df00fffffffb000000160053006c0061006d0054006f006f006c0062006f00780100000222000001850000018500fffffffb0000000a0049006d006100670065000000003d000004c20000000000000000000003730000036a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000002000000000000015d000004c2fc0200000005fb000000100044006900730070006c006100790073010000003d0000011a000000c900fffffffb0000000a00560069006500770073010000015d000000d8000000a400fffffffb0000000c00430061006d006500720061020000041f000000c8000001c3000000d6fb0000000c00540065006c0065006f0070010000023b0000013e0000013e00fffffffb0000000a00440065007000740068010000037f000001800000001600ffffff00000001000001ce000004c2fc0200000003fb00000014004e0061007600690067006100740069006f006e010000003d0000029d000001df00fffffffb000000160053006c0061006d0054006f006f006c0062006f007801000002e00000021f0000018500fffffffb0000000a0049006d006100670065000000003d000004c20000000000000000000006c9000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   SlamToolbox:
     collapsed: false
   Teleop:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1706
-  X: 134
-  Y: 116
+  Width: 2560
+  X: 0
+  Y: 68

mote_bringup/config/nav2_params.yaml

@@ -107,7 +107,7 @@ local_costmap:
       height: 3
       resolution: 0.05
       robot_radius: 0.12
-      plugins: ["obstacle_layer", "inflation_layer"]
+      plugins: ["obstacle_layer", "camera_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
@@ -122,6 +122,54 @@ local_costmap:
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
+      # Monocular-depth obstacles (mote_perception/depth_obstacle_node): the low,
+      # near things the single lidar plane misses (cables, thresholds, chair and
+      # table legs, a clothes-horse cross-bar). A separate layer, kept off the global
+      # costmap: near-band, reactive, and never allowed to clear the lidar layer.
+      # It self-clears from its own dense observations (a stale mark is raytraced away
+      # next frame). Caveat: a phantom over open floor with nothing above-floor behind
+      # it in range gets no clearing ray until the rolling window scrolls past it as
+      # the robot moves; if that proves visible on the robot, swap in
+      # spatio_temporal_voxel_layer (time-decay + frustum clearing). The cloud carries
+      # leveled base_footprint coords, but sensor_frame pins the clearing-ray origin to
+      # the real camera height so the rays descend onto the floor rather than rising
+      # through the low-obstacle band.
+      camera_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        footprint_clearing_enabled: True
+        max_obstacle_height: 0.2
+        origin_z: 0.0
+        z_resolution: 0.025
+        z_voxels: 8
+        unknown_threshold: 15
+        mark_threshold: 0
+        combination_method: 1
+        publish_voxel_map: False
+        observation_sources: camera
+        camera:
+          topic: /camera_obstacles
+          sensor_frame: camera_optical_link
+          data_type: "PointCloud2"
+          marking: True
+          clearing: True
+          min_obstacle_height: 0.02
+          # Go-under gate: anything whose lowest point clears this height is passable
+          # overhead the robot drives under (a chair seat, a tabletop) and must not
+          # mark. Because this is a 3D voxel layer, the legs of that furniture (which
+          # reach the floor) still mark, so the robot avoids the legs but paths
+          # through the gap beneath. Set to the robot's height + a margin: 0.18 m for
+          # the current ~0.13 m chassis (no arm). WITH THE ARM the robot is ~0.30 m --
+          # raise this to ~0.35 m AND raise the voxel-grid top (z_voxels * z_resolution,
+          # e.g. z_resolution 0.05 + z_voxels 8 = 0.40 m); the go-under benefit shrinks
+          # at that height (a 0.30 m robot no longer fits under most chair seats).
+          max_obstacle_height: 0.18
+          obstacle_max_range: 1.2
+          obstacle_min_range: 0.0
+          raytrace_max_range: 1.3
+          raytrace_min_range: 0.0
+          expected_update_rate: 0.0
+          inf_is_valid: False
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0