oomwoo_clean/nav: add localization:=truth (perfect sim debug pose)

The sim's odometry is noise-free and world-referenced, so localization
error is not odometry drift -- it is AMCL wandering as it fits scans to a
map that may not match the robot's LiDAR. localization:=truth (sim only)
skips AMCL and publishes a static identity map->odom; map->base then tracks
the true pose exactly, forever, isolating navigation/map/tuning from
localization. localization:=amcl (default) is unchanged.

Verified headless: truth -> map->odom identity, map->base = true spawn
(-2,-0.5), goal SUCCEEDED; amcl -> localizes ~2s, goal SUCCEEDED.

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>

Author: kaiaai

README.md

@@ -112,6 +112,17 @@ unknown — seed it with RViz's **2D Pose Estimate**) and leave `coverage` off:
 coverage marking is ground-truth based, so it works in sim only (a real robot
 needs a belief-based estimator, not built yet).
 
+**Debugging with perfect localization.** The sim's odometry is noise-free, so
+you can take AMCL (and any map/scan mismatch) out of the picture entirely:
+
+```bash
+ros2 launch oomwoo_clean nav.launch.py use_sim_time:=true localization:=truth
+```
+
+`localization:=truth` (sim only) skips AMCL and publishes a static identity
+`map→odom`, so `map→base` tracks the true pose exactly — useful for isolating
+navigation, map, or tuning issues from localization error.
+
 ## Quickstart — reproduce the regressions
 
 Copy-paste. Needs Docker on a native **x86-64 Linux** box (not ARM / not an

src/oomwoo_clean/launch/nav.launch.py

@@ -12,24 +12,27 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 """
-Nav2 + AMCL on an existing map, for the cleaning-with-map workflow.
+Nav2 on an existing map, for the cleaning-with-map workflow.
 
 Localization and navigation ONLY -- no Gazebo, no RViz -- so it composes with a
 separately-launched robot source (`oomwoo_gazebo world.launch.py` in sim, or
 `oomwoo_bringup physical.launch.py` on a real robot) and a separate viewer
 (`oomwoo_clean_ui cleaning_debug.launch.py`). Uses the selected robot's own
 navigation.yaml, so the same command follows `kaia use <robot>`.
 
-auto_localize:=true (default) seeds AMCL at the spawn pose so the sim localizes
-itself -- no manual RViz 2D Pose Estimate. Set it false on a real robot (you do
-not know the pose there) and seed with the 2D Pose Estimate instead.
-
-coverage:=true adds the ground-truth coverage meter that marks the floor clean
-as the robot drives. That meter is ground-truth based, so it is SIM ONLY; on a
-real robot leave coverage off (a belief-based estimator does not exist yet). Both
-auto_localize and coverage use the spawn pose, which must match the sim's
-(world.launch.py defaults), so the robot localizes and the covered cells line up
-with the map.
+localization:=
+  amcl  (default) -- Nav2 AMCL scan-matches against the map. Real, but only as
+        good as the map: if the loaded map does not match the robot's LiDAR,
+        AMCL's estimate wanders. auto_localize:=true seeds it at the spawn pose
+        (no manual RViz 2D Pose Estimate); set false on a real robot.
+  truth -- DEBUG, SIM ONLY. No AMCL: publish a static map->odom at the spawn
+        pose. The sim's odometry is noise-free, so map->base then tracks the
+        true pose exactly, forever. Use it to take localization error out of the
+        picture and debug navigation / the map / tuning on their own.
+
+coverage:=true adds the ground-truth coverage meter (sim only). auto_localize,
+coverage and the truth transform all use the spawn pose, which must match the
+sim's (world.launch.py defaults) so everything lines up with the map.
 """
 
 import os
@@ -47,7 +50,41 @@
 from launch_ros.parameter_descriptions import ParameterValue
 
 
-def _nav(context, robot_model, map_yaml, use_sim_time):
+def _seed_node(xv, yv, yawv):
+    # (Re)publish /initialpose until AMCL localizes, then exit. The robot's AMCL
+    # does not self-seed, so this is what puts map->odom on the tree in sim.
+    return Node(
+        package='oomwoo_sim_support', executable='initialpose_pub',
+        name='initialpose_pub', output='screen',
+        parameters=[{'use_sim_time': True, 'reseed_after_sec': 1.0,
+                     'x': xv, 'y': yv, 'yaw': yawv}])
+
+
+def _truth_localization(mapf, use_sim):
+    # Perfect debug localization: a static IDENTITY map->odom. The sim's
+    # odometry is noise-free AND world-referenced (odom->base already reports
+    # the true world pose), and the map is world-aligned, so map == odom == world
+    # -- map->base then equals the true pose for all time, no AMCL, no
+    # scan-vs-map fitting.
+    map_server = Node(
+        package='nav2_map_server', executable='map_server', name='map_server',
+        output='screen',
+        parameters=[{'yaml_filename': mapf, 'use_sim_time': use_sim,
+                     'topic_name': 'map', 'frame_id': 'map'}])
+    lifecycle = Node(
+        package='nav2_lifecycle_manager', executable='lifecycle_manager',
+        name='lifecycle_manager_localization', output='screen',
+        parameters=[{'use_sim_time': use_sim, 'autostart': True,
+                     'node_names': ['map_server']}])
+    map_odom = Node(
+        package='tf2_ros', executable='static_transform_publisher',
+        name='map_odom_truth', output='screen',
+        arguments=['--frame-id', 'map', '--child-frame-id', 'odom'])
+    return [map_server, lifecycle, map_odom]
+
+
+def _nav(context, robot_model, map_yaml, use_sim_time, localization,
+         auto_localize, x0, y0, yaw0):
     model = context.perform_substitution(robot_model)
     if not model:
         try:
@@ -57,16 +94,32 @@ def _nav(context, robot_model, map_yaml, use_sim_time):
             model = 'oomwoo_one'
     params = os.path.join(
         get_package_share_directory(model), 'config', 'navigation.yaml')
-    bringup = os.path.join(
-        get_package_share_directory('nav2_bringup'),
-        'launch', 'bringup_launch.py')
-    return [IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(bringup),
-        launch_arguments={
-            'map': context.perform_substitution(map_yaml),
-            'use_sim_time': context.perform_substitution(use_sim_time),
-            'params_file': params,
-            'slam': 'False'}.items())]
+    launch_dir = os.path.join(
+        get_package_share_directory('nav2_bringup'), 'launch')
+    mapf = context.perform_substitution(map_yaml)
+    sim = context.perform_substitution(use_sim_time)
+    xv = float(context.perform_substitution(x0))
+    yv = float(context.perform_substitution(y0))
+    yawv = float(context.perform_substitution(yaw0))
+
+    if context.perform_substitution(localization) == 'truth':
+        nav = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                os.path.join(launch_dir, 'navigation_launch.py')),
+            launch_arguments={'use_sim_time': sim,
+                              'params_file': params}.items())
+        return _truth_localization(mapf, sim.lower() == 'true') + [nav]
+
+    # amcl (default): the full Nav2 bringup (map_server + AMCL + navigation)
+    bringup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_dir, 'bringup_launch.py')),
+        launch_arguments={'map': mapf, 'use_sim_time': sim,
+                          'params_file': params, 'slam': 'False'}.items())
+    actions = [bringup]
+    if context.perform_substitution(auto_localize).lower() in ('true', '1'):
+        actions.append(_seed_node(xv, yv, yawv))
+    return actions
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -90,12 +143,14 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument('robot_model', default_value=''),
         DeclareLaunchArgument('map', default_value=default_map),
         DeclareLaunchArgument('use_sim_time', default_value='true'),
-        # auto-seed AMCL at the spawn pose (sim); false on a real robot
+        # 'amcl' (real) or 'truth' (perfect static map->odom, sim debug)
+        DeclareLaunchArgument('localization', default_value='amcl',
+                              choices=['amcl', 'truth']),
+        # auto-seed AMCL at the spawn pose (amcl mode, sim); false on a robot
         DeclareLaunchArgument('auto_localize', default_value='true'),
         # ground-truth coverage marking: sim only, off by default
         DeclareLaunchArgument('coverage', default_value='false'),
-        # must match the sim spawn (world.launch.py defaults) so the robot
-        # localizes and coverage aligns
+        # must match the sim spawn (world.launch.py defaults)
         DeclareLaunchArgument('x_pose', default_value='-2.0'),
         DeclareLaunchArgument('y_pose', default_value='-0.5'),
         DeclareLaunchArgument('yaw', default_value='0.0'),
@@ -104,7 +159,10 @@ def generate_launch_description() -> LaunchDescription:
     nav = OpaqueFunction(function=_nav, args=[
         LaunchConfiguration('robot_model'),
         LaunchConfiguration('map'),
-        LaunchConfiguration('use_sim_time')])
+        LaunchConfiguration('use_sim_time'),
+        LaunchConfiguration('localization'),
+        LaunchConfiguration('auto_localize'),
+        x0, y0, yaw0])
 
     with_coverage = IfCondition(LaunchConfiguration('coverage'))
     ground_truth = Node(
@@ -125,18 +183,4 @@ def generate_launch_description() -> LaunchDescription:
                     ('ground_truth/pose', '/ground_truth/pose'),
                     ('cleaning_active', '/coverage_planner/cleaning_active')])
 
-    # Seed AMCL at the spawn pose so the sim localizes without a manual RViz
-    # 2D Pose Estimate. initialpose_pub (re)publishes /initialpose until AMCL
-    # localizes, then exits; the robot's AMCL does not self-seed, so this is
-    # what puts map->odom on the tree.
-    seed = Node(
-        package='oomwoo_sim_support', executable='initialpose_pub',
-        name='initialpose_pub', output='screen',
-        condition=IfCondition(LaunchConfiguration('auto_localize')),
-        parameters=[{'use_sim_time': True, 'reseed_after_sec': 1.0,
-                     'x': ParameterValue(x0, value_type=float),
-                     'y': ParameterValue(y0, value_type=float),
-                     'yaw': ParameterValue(yaw0, value_type=float)}])
-
-    return LaunchDescription(
-        args + [nav, ground_truth, coverage_meter, seed])
+    return LaunchDescription(args + [nav, ground_truth, coverage_meter])