修了一下障碍物相关

src/navigation/obstacle_nav2/config/nav2_params.yaml

@@ -76,13 +76,13 @@ controller_server:
       time_steps: 36
       model_dt: 0.05
       batch_size: 1000
-      vx_std: 0.75
+      vx_std: 0.22
       vy_std: 0.0
       wz_std: 0.4
-      vx_max: 1.75
+      vx_max: 0.75
       vx_min: -0.75
       vy_max: 0.0
-      wz_max: 1.5
+      wz_max: 2.0
       iteration_count: 1
       temperature: 0.3
       gamma: 0.015
@@ -109,9 +109,9 @@ controller_server:
         cost_weight: 3.0
         threshold_to_consider: 0.5
       PreferForwardCritic:
-        enabled: false
+        enabled: true
         cost_power: 1
-        cost_weight: 0.0
+        cost_weight: 3.0
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
@@ -246,10 +246,10 @@ planner_server:
       analytic_expansion_ratio: 3.5
       analytic_expansion_max_length: 3.0
       minimum_turning_radius: 0.40
-      reverse_penalty: 1.0
+      reverse_penalty: 1.9
-      change_penalty: 0.0
+      change_penalty: 1.0
       non_straight_penalty: 1.2
-      cost_penalty: 2.0
+      cost_penalty: 4.0
       retrospective_penalty: 0.015
       # 5 m covers the rolling planning horizon without the startup and memory
       # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
@@ -324,10 +324,10 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [1.75, 0.0, 1.5]
+    max_velocity: [0.75, 0.0, 2.5]
-    min_velocity: [-0.75, 0.0, -1.5]
+    min_velocity: [-0.75, 0.0, -2.5]
     max_accel: [2.5, 0.0, 3.2]
-    max_decel: [-2.5, 0.0, -3.2]
+    max_decel: [-0.5, 0.0, -0.5]
     odom_topic: /odom_combined
     odom_duration: 0.1
     deadband_velocity: [0.03, 0.0, 0.03]

src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-205824

@@ -0,0 +1,334 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: odom
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.75
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 1.75
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: false
+        cost_power: 1
+        cost_weight: 0.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 10
+      height: 10
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.0
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.75, 0.0, 1.5]
+    min_velocity: [-0.75, 0.0, -1.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/navigation/obstacle_nav2/config/nav2_params.yaml.bak.20260724-210811

@@ -0,0 +1,334 @@
+# ============================================================================
+# nav2_params.yaml — Odometry-only obstacle navigation
+#
+# No static map, no AMCL, no SLAM.
+# Both costmaps are rolling windows in odom. The launch file can rewrite every
+# global_frame leaf when a different connected odometry frame is required.
+# Global planner: Smac Hybrid A* (Reeds-Shepp)
+# Local controller: MPPI (Ackermann)
+# ============================================================================
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: odom
+    robot_base_frame: base_footprint
+    odom_topic: /odom_combined
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # Injected by obstacle_nav2.launch.py from this package's share directory.
+    default_nav_to_pose_bt_xml: ""
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 36
+      model_dt: 0.05
+      batch_size: 1000
+      vx_std: 0.55
+      vy_std: 0.0
+      wz_std: 0.4
+      vx_max: 1.75
+      vx_min: -0.75
+      vy_max: 0.0
+      wz_max: 1.5
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: false
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      transform_tolerance: 0.5
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 10
+      height: 10
+      resolution: 0.05
+      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      track_unknown_space: false
+      plugins: ["obstacle_array_layer", "inflation_layer"]
+      obstacle_array_layer:
+        plugin: "obstacle_nav2::ObstacleArrayLayer"
+        enabled: true
+        topic: /obstacles
+        obstacle_timeout: 0.5
+        transform_tolerance: 0.2
+        default_obstacle_radius: 0.05
+        minimum_obstacle_radius: 0.02
+        maximum_obstacle_radius: 0.50
+        extra_inflation: 0.02
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: false
+      max_iterations: 1000000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 3.5
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.25
+      change_penalty: 0.0
+      non_straight_penalty: 1.2
+      cost_penalty: 2.0
+      retrospective_penalty: 0.015
+      # 5 m covers the rolling planning horizon without the startup and memory
+      # cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.3
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 2
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_tolerance: 0.5
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: False
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [1.75, 0.0, 1.5]
+    min_velocity: [-0.75, 0.0, -1.5]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: /odom_combined
+    odom_duration: 0.1
+    deadband_velocity: [0.03, 0.0, 0.03]
+    velocity_timeout: 1.0

src/origincar_base/CMakeLists.txt

@@ -67,6 +67,43 @@ set(origincar_base_node_SRCS
 add_executable(origincar_base_node src/origincar_base.cpp)
 ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
 
+add_library(wall_fit_core STATIC src/wall_fit_core.cpp)
+target_include_directories(wall_fit_core PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+add_library(wall_kalman_filter STATIC src/wall_kalman_filter.cpp)
+target_include_directories(wall_kalman_filter PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(wall_kalman_filter wall_fit_core)
+
+target_link_libraries(origincar_base_node wall_kalman_filter wall_fit_core)
+
+add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp)
+target_link_libraries(wall_fit_calibrator wall_fit_core)
+ament_target_dependencies(wall_fit_calibrator rclcpp sensor_msgs nav_msgs)
+
+add_executable(wall_localization_node src/wall_localization_node.cpp)
+target_link_libraries(wall_localization_node wall_fit_core)
+ament_target_dependencies(wall_localization_node rclcpp sensor_msgs nav_msgs geometry_msgs tf2_ros)
+
+add_executable(wall_preview_server src/wall_preview_server.cpp)
+target_link_libraries(wall_preview_server wall_fit_core pthread)
+ament_target_dependencies(wall_preview_server rclcpp sensor_msgs nav_msgs geometry_msgs)
+
+if(BUILD_TESTING)
+  add_executable(wall_fit_core_test test/wall_fit_core_test.cpp)
+  target_link_libraries(wall_fit_core_test wall_fit_core)
+  add_test(NAME wall_fit_core_test COMMAND wall_fit_core_test)
+
+  add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp)
+  target_link_libraries(wall_kalman_filter_test wall_kalman_filter)
+  add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test)
+endif()
+
 #add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
 #ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg)
 
@@ -77,6 +114,9 @@ install(PROGRAMS
 
 install(TARGETS
 origincar_base_node
+wall_fit_calibrator
+wall_localization_node
+wall_preview_server
 
 DESTINATION lib/${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
@@ -85,7 +125,7 @@ DESTINATION lib/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY launch  config 
+  DIRECTORY launch  config preview
   DESTINATION share/${PROJECT_NAME}
 )
 ament_package()

src/origincar_base/include/origincar_base/origincar_base.h

@@ -4,8 +4,12 @@
 #include <memory>
 #include <inttypes.h>
 #include <array>
+#include <mutex>
+#include <vector>
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "origincar_base/wall_fit_core.hpp"
+#include "origincar_base/wall_kalman_filter.hpp"
 #include <csignal>
 #include <thread>
 
@@ -35,19 +39,19 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
 #include "origincar_msg/msg/data.hpp"
-#include "origincar_msg/msg/sign.hpp" // 匹配信号发送
+#include "origincar_msg/msg/sign.hpp"
 #include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 using namespace std;
 
-#define SEND_DATA_CHECK 1	 // Send data check flag bits //发送数据校验标志位
+#define SEND_DATA_CHECK 1
-#define READ_DATA_CHECK 0	 // Receive data to check flag bits //接收数据校验标志位
+#define READ_DATA_CHECK 0
-#define FRAME_HEADER 0X7B	 // Frame head //帧头
+#define FRAME_HEADER 0X7B
-#define FRAME_TAIL 0X7D		 // Frame tail //帧尾
+#define FRAME_TAIL 0X7D
-#define RECEIVE_DATA_SIZE 24 // The length of the data sent by the lower computer //下位机发送过来的数据的长度
+#define RECEIVE_DATA_SIZE 24
-#define SEND_DATA_SIZE 11	 // The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
+#define SEND_DATA_SIZE 11
-#define PI 3.1415926f		 // PI //圆周率
+#define PI 3.1415926f
-
 #define GYROSCOPE_RATIO 0.00026644f
 
 #define ACCEl_RATIO 1671.84f
@@ -145,6 +149,8 @@ private:
 	void Publish_Voltage();
 	void Publish_GyroDebug();
 	auto createQuaternionMsgFromYaw(double yaw);
+	void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
+	void Apply_Wall_Update();
 
 	bool Get_Sensor_Data();
 	unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
@@ -158,24 +164,28 @@ private:
 	{
 		auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
 
-		// 设置头信息
+		// Reset pose header
 		msg->header.stamp = this->now();
 		msg->header.frame_id = "odom_combined";
 
-		// 重置位姿
+		// Reset pose
 		msg->pose.pose.position.x = 0.54;
 		msg->pose.pose.position.y = 0.2;
 		msg->pose.pose.orientation.w = 1.0;
 
-		// 设置关键协方差
+		// Reset covariance
 		std::array<double, 36> cov = {0};
 		cov[0] = 0.5;  // x
 		cov[7] = 0.5;  // y
 		cov[35] = 0.5; // yaw
 		msg->pose.covariance = cov;
 
-		// 发布重置指令
+		// Publish reset command
 		pose_pub_->publish(*msg);
+		if (wall_filter_)
+		{
+			wall_filter_->reset({0.56, 0.18, 0.0});
+		}
 		RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!");
 	}
 
@@ -184,6 +194,7 @@ private:
 	float Sampling_Time;
 	rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr Cmd_Vel_Sub;
 	rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr Akm_Cmd_Vel_Sub;
+	rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr Scan_Sub;
 
 	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher;
 	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher;
@@ -212,8 +223,10 @@ private:
 	rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr Sign_Switch_Sub;
 
 	string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
+	string scan_topic_, wall_config_path_, combined_odom_topic_;
 	bool publish_tf_;
 	double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
+	int wall_scan_stride_;
 	std::string cmd_vel;
 	int serial_baud_rate;
 	RECEIVE_DATA Receive_Data;
@@ -236,6 +249,14 @@ private:
 	size_t gyro_z_median_count_;
 	bool gyro_z_low_pass_initialized_;
 	size_t count_;
+	::origincar_wall::WallFitConfig wall_fit_config_;
+	std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_;
+	::origincar_wall::Pose2D laser_pose_;
+	std::mutex wall_scan_mutex_;
+	std::vector<::origincar_wall::WallPoint> latest_scan_points_;
+	bool has_latest_scan_;
+	bool latest_scan_consumed_;
 };
 
 #endif //_ORIGINCAR_BASE_H_
+

src/origincar_base/include/origincar_base/wall_fit_core.hpp

@@ -0,0 +1,128 @@
+#ifndef ORIGINCAR_BASE_WALL_FIT_CORE_HPP_
+#define ORIGINCAR_BASE_WALL_FIT_CORE_HPP_
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace origincar_wall
+{
+
+struct WallPoint
+{
+  double x;
+  double y;
+};
+
+struct Pose2D
+{
+  double x;
+  double y;
+  double theta;
+};
+
+struct WallBounds
+{
+  double x_min;
+  double x_max;
+  double y_min;
+  double y_max;
+};
+
+struct WallLine
+{
+  std::string name;
+  double a;
+  double b;
+  double c;
+  std::string color;
+};
+
+struct ReferencePoint
+{
+  std::string label;
+  WallPoint image;
+  WallPoint map;
+};
+
+struct CoordinateFrame
+{
+  std::string origin;
+  std::string origin_note;
+  double width_m;
+  double height_m;
+  int image_width_px;
+  int image_height_px;
+  int pixels_per_meter;
+  WallPoint p_image_px;
+  std::string odom_zero_reference;
+  WallPoint odom_zero_map;
+};
+
+struct WallFitConfig
+{
+  CoordinateFrame coordinate_frame;
+  std::vector<WallLine> walls;
+  std::map<std::string, ReferencePoint> reference_points;
+  std::string generated_at;
+};
+
+struct MatchedWall
+{
+  std::string name;
+  int point_count;
+  double observed;
+  double target;
+  double correction;
+  double mean_error;
+};
+
+struct LocalizationQuality
+{
+  int wall_count;
+  double mean_error;
+  bool has_mean_error;
+};
+
+struct LocalizationResult
+{
+  bool ok;
+  Pose2D pose;
+  std::vector<WallPoint> scan_map;
+  std::vector<MatchedWall> matched_walls;
+  LocalizationQuality quality;
+  std::string message;
+};
+
+struct WallFitCapture
+{
+  std::string key;
+  WallPoint map;
+  Pose2D odom_pose;
+  std::vector<WallPoint> scan_points;
+};
+
+WallFitConfig defaultWallFitConfig();
+WallBounds wallBounds(const WallFitConfig &config);
+double normalizeAngle(double angle);
+Pose2D predictPoseFromOdomDelta(const Pose2D &previous_wall_pose,
+                                const Pose2D &previous_odom_pose,
+                                const Pose2D &current_odom_pose);
+std::vector<WallPoint> transformScanPointsToMap(const std::vector<WallPoint> &scan_points,
+                                                const Pose2D &pose);
+std::vector<WallPoint> transformScanPointsToPoseFrame(const std::vector<WallPoint> &scan_points,
+                                                      const Pose2D &laser_pose);
+std::vector<WallPoint> mapPointsToScan(const std::vector<WallPoint> &map_points,
+                                       const Pose2D &pose);
+LocalizationResult localizeFromScan(const std::vector<WallPoint> &scan_points,
+                                    const WallFitConfig &config,
+                                    const Pose2D &predicted_pose);
+WallFitConfig fitWallConfigFromCaptures(const std::vector<WallFitCapture> &captures,
+                                        const WallFitConfig &base_config);
+WallFitConfig loadWallFitConfig(const std::string &path);
+void saveWallFitConfig(const std::string &path, const WallFitConfig &config);
+std::string wallFitConfigToJson(const WallFitConfig &config);
+
+}  // namespace origincar_wall
+
+#endif  // ORIGINCAR_BASE_WALL_FIT_CORE_HPP_

src/origincar_base/include/origincar_base/wall_kalman_filter.hpp

@@ -0,0 +1,36 @@
+#ifndef ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
+#define ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
+
+#include "origincar_base/wall_fit_core.hpp"
+
+namespace origincar_wall
+{
+
+struct WallKalmanNoise
+{
+  double x;
+  double y;
+  double theta;
+};
+
+class WallKalmanFilter
+{
+public:
+  explicit WallKalmanFilter(const Pose2D &initial_pose);
+
+  void reset(const Pose2D &pose);
+  void predict(double vx, double vy, double yaw_rate, double dt);
+  void correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise);
+
+  Pose2D pose() const;
+  WallKalmanNoise covariance() const;
+
+private:
+  Pose2D pose_;
+  WallKalmanNoise covariance_;
+  WallKalmanNoise process_noise_per_second_;
+};
+
+}  // namespace origincar_wall
+
+#endif  // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_

src/origincar_base/launch/base_serial.launch.py

@@ -6,8 +6,13 @@ def generate_launch_description():
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
          'serial_baud_rate': 115200,
-         'robot_frame_id': 'base_link',
+         'robot_frame_id': 'base_footprint',
          'odom_frame_id': 'odom',
+         'combined_odom_topic': 'odom_combined',
+         'scan_topic': '/scan',
+         'wall_config_path': '/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json',
+         'publish_tf': True,
+         'wall_scan_stride': 2,
          'cmd_vel': 'cmd_vel',
          'akm_cmd_vel': 'none',
          'product_number': 0,

src/origincar_base/launch/origincar_bringup.launch.py

@@ -1,99 +1,87 @@
 import os
 from pathlib import Path
-import launch
+
-from launch.actions import SetEnvironmentVariable
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import (DeclareLaunchArgument, GroupAction,
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import PushRosNamespace
+from launch_ros.actions import Node
-import launch_ros.actions
+from launch_ros.parameter_descriptions import ParameterValue
-from launch.conditions import UnlessCondition
+
 
 def generate_launch_description():
-    # Get the launch directory
     bringup_dir = get_package_share_directory('origincar_base')
     launch_dir = os.path.join(bringup_dir, 'launch')
-    ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml')
     imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml')
 
-    
     carto_slam = LaunchConfiguration('carto_slam', default='false')
-    carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false')
+    akmcar = LaunchConfiguration('akmcar', default='true')
+    wall_preview = LaunchConfiguration('wall_preview', default='false')
+    wall_preview_port = LaunchConfiguration('wall_preview_port', default='8772')
 
-    akmcar = LaunchConfiguration('akmcar', default='false')
-    akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true',
-        description='阿克曼底盘模式 (true=阿克曼, false=差速)')
-            
     origincar_base = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')),
+        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')),
-            launch_arguments={'akmcar': akmcar}.items(),
+        launch_arguments={'akmcar': akmcar}.items(),
     )
 
-    choose_car = IncludeLaunchDescription(
+    robot_mode_description = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
+        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
     )
 
-    
+    base_to_gyro = Node(
-    
+        package='tf2_ros',
-
+        executable='static_transform_publisher',
-    base_to_gyro = launch_ros.actions.Node(
+        name='base_to_gyro',
-            package='tf2_ros', 
+        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'gyro_link'],
-            executable='static_transform_publisher', 
-            name='base_to_gyro',
-            arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'],
-    )
-    
-    link_to_laser = launch_ros.actions.Node(
-            package='tf2_ros', 
-            executable='static_transform_publisher', 
-            name='link_to_laser',
-            arguments=['0', '0', '0','0', '0','0','base_link','laser'],
     )
 
-    imu_filter_node =  launch_ros.actions.Node(
+    link_to_laser = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='link_to_laser',
+        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'laser'],
+    )
+
+    imu_filter_node = Node(
         package='imu_filter_madgwick',
         executable='imu_filter_madgwick_node',
-        parameters=[imu_config]
+        parameters=[imu_config],
+    )
+
+    wall_preview_server = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'wall_localization.launch.py')),
+        condition=IfCondition(wall_preview),
+        launch_arguments={
+            'port': wall_preview_port,
+        }.items(),
     )
-    
-    robot_ekf = launch_ros.actions.Node(
-            condition=UnlessCondition(carto_slam),
-            package='robot_localization', 
-            executable='ekf_node', 
-            parameters=[ekf_config],
-            remappings=[("odometry/filtered", "odom_combined")]
-            )
-                              
-    # 从 URDF 生成 robot_description，供 joint_state_publisher 和 robot_state_publisher 共用
-    from launch_ros.parameter_descriptions import ParameterValue
-    from launch.substitutions import Command
 
     robot_description = ParameterValue(
-        Command(['xacro ', os.path.join(
+        Command([
-            get_package_share_directory('origincar_description'),
+            'xacro ',
-            'urdf', 'origincar.urdf')]),
+            os.path.join(get_package_share_directory('origincar_description'), 'urdf', 'origincar.urdf'),
-        value_type=str)
+        ]),
-
+        value_type=str,
-    joint_state_publisher_node = launch_ros.actions.Node(
-            package='joint_state_publisher', 
-            executable='joint_state_publisher', 
-            name='joint_state_publisher',
-            parameters=[{'robot_description': robot_description}],
     )
 
-    ld = LaunchDescription()
+    joint_state_publisher_node = Node(
-
+        package='joint_state_publisher',
-    ld.add_action(carto_slam_dec)
+        executable='joint_state_publisher',
-    ld.add_action(akmcar_dec)
+        name='joint_state_publisher',
-    ld.add_action(origincar_base)
+        parameters=[{'robot_description': robot_description}],
-    ld.add_action(base_to_gyro)
+    )
-    ld.add_action(joint_state_publisher_node)
-    ld.add_action(choose_car)
-    ld.add_action(imu_filter_node)
-    ld.add_action(robot_ekf)
-
-    return ld
 
+    return LaunchDescription([
+        DeclareLaunchArgument('carto_slam', default_value='false'),
+        DeclareLaunchArgument('akmcar', default_value='true'),
+        DeclareLaunchArgument('wall_preview', default_value='false'),
+        DeclareLaunchArgument('wall_preview_port', default_value='8772'),
+        origincar_base,
+        base_to_gyro,
+        joint_state_publisher_node,
+        robot_mode_description,
+        imu_filter_node,
+        wall_preview_server,
+    ])

src/origincar_base/launch/wall_localization.launch.py

@@ -0,0 +1,33 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    scan_topic = LaunchConfiguration('scan_topic')
+    combined_odom_topic = LaunchConfiguration('combined_odom_topic')
+    wall_config = LaunchConfiguration('wall_config')
+    port = LaunchConfiguration('port')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('scan_topic', default_value='/scan'),
+        DeclareLaunchArgument('combined_odom_topic', default_value='/odom_combined'),
+        DeclareLaunchArgument(
+            'wall_config',
+            default_value='/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json'),
+        DeclareLaunchArgument('port', default_value='8772'),
+
+        Node(
+            package='origincar_base',
+            executable='wall_preview_server',
+            name='wall_preview_server',
+            output='screen',
+            parameters=[{
+                'scan_topic': scan_topic,
+                'combined_odom_topic': combined_odom_topic,
+                'config_path': wall_config,
+                'port': port,
+            }],
+        ),
+    ])

src/origincar_base/preview/wall_localization.html

@@ -0,0 +1,275 @@
+<!doctype html>
+<html lang="zh-CN">
+<head>
+  <meta charset="utf-8">
+  <meta name="viewport" content="width=device-width, initial-scale=1">
+  <title>Wall Localization Preview</title>
+  <style>
+    :root {
+      --bg: #f5f7fb;
+      --panel: #ffffff;
+      --ink: #111827;
+      --muted: #64748b;
+      --line: #d8dee9;
+      --wall-pose: #2563eb;
+      --scan: #14532d;
+    }
+    * { box-sizing: border-box; }
+    body {
+      margin: 0;
+      background: var(--bg);
+      color: var(--ink);
+      font-family: system-ui, -apple-system, BlinkMacSystemFont, "Segoe UI", sans-serif;
+    }
+    main {
+      display: grid;
+      grid-template-columns: minmax(0, 1fr) 320px;
+      gap: 16px;
+      min-height: 100vh;
+      padding: 16px;
+    }
+    .stage, aside {
+      background: var(--panel);
+      border: 1px solid var(--line);
+      border-radius: 8px;
+    }
+    .stage {
+      display: grid;
+      place-items: center;
+      min-width: 0;
+      padding: 12px;
+    }
+    canvas {
+      width: min(82vh, 100%);
+      max-width: 920px;
+      aspect-ratio: 1 / 1;
+      border: 1px solid var(--line);
+      background: #ffffff;
+    }
+    aside {
+      padding: 14px;
+      display: flex;
+      flex-direction: column;
+      gap: 12px;
+    }
+    h1 {
+      margin: 0;
+      font-size: 18px;
+      line-height: 1.2;
+    }
+    .muted { color: var(--muted); font-size: 13px; }
+    .metric {
+      display: grid;
+      gap: 3px;
+      padding: 10px 0;
+      border-top: 1px solid var(--line);
+    }
+    .metric strong { font-size: 13px; overflow-wrap: anywhere; }
+    label {
+      display: flex;
+      align-items: center;
+      gap: 8px;
+      font-size: 14px;
+    }
+    .legend {
+      display: grid;
+      gap: 8px;
+      font-size: 13px;
+    }
+    .legend span {
+      display: flex;
+      align-items: center;
+      gap: 8px;
+    }
+    .swatch {
+      width: 12px;
+      height: 12px;
+      border-radius: 999px;
+      background: var(--c);
+      display: inline-block;
+    }
+    @media (max-width: 900px) {
+      main { grid-template-columns: 1fr; }
+      canvas { width: 100%; }
+    }
+  </style>
+</head>
+<body>
+  <main>
+    <section class="stage">
+      <canvas id="mapCanvas" width="900" height="900" aria-label="wall localization map"></canvas>
+      <img id="mapImage" src="/map_reference.png" alt="" hidden>
+    </section>
+    <aside>
+      <div>
+        <h1>墙定位预览</h1>
+        <div class="muted">lower-left map frame, runtime only</div>
+      </div>
+      <label><input id="showMap" type="checkbox" checked>叠加地图</label>
+      <label><input id="showScan" type="checkbox">显示 /scan</label>
+      <div class="legend">
+        <span><i class="swatch" style="--c:var(--wall-pose)"></i>/odom_combined</span>
+        <span><i class="swatch" style="--c:var(--scan)"></i>/scan</span>
+      </div>
+      <div class="metric"><span class="muted">status</span><strong id="status">connecting</strong></div>
+      <div class="metric"><span class="muted">odom_combined</span><strong id="combinedPose">-</strong></div>
+      <div class="metric"><span class="muted">wall bounds</span><strong id="boundsText">-</strong></div>
+    </aside>
+  </main>
+
+  <script>
+    const canvas = document.getElementById('mapCanvas');
+    const ctx = canvas.getContext('2d');
+    const mapImage = document.getElementById('mapImage');
+    const showMap = document.getElementById('showMap');
+    const showScan = document.getElementById('showScan');
+    const statusEl = document.getElementById('status');
+    const combinedPoseEl = document.getElementById('combinedPose');
+    const boundsTextEl = document.getElementById('boundsText');
+    let state = {wall_fit: null, odom_combined: null, scan_map: null};
+
+    function wallByName(name) {
+      const walls = state.wall_fit && Array.isArray(state.wall_fit.walls) ? state.wall_fit.walls : [];
+      return walls.find(wall => wall.name === name);
+    }
+
+    function computeBounds() {
+      const left = wallByName('left');
+      const right = wallByName('right');
+      const bottom = wallByName('bottom');
+      const top = wallByName('top');
+      if (!left || !right || !bottom || !top) return {x_min: 0, x_max: 5, y_min: 0, y_max: 5};
+      const bounds = {
+        x_min: -Number(left.c) / Number(left.a),
+        x_max: -Number(right.c) / Number(right.a),
+        y_min: -Number(bottom.c) / Number(bottom.b),
+        y_max: -Number(top.c) / Number(top.b)
+      };
+      if (Object.values(bounds).some(value => !Number.isFinite(value))) return {x_min: 0, x_max: 5, y_min: 0, y_max: 5};
+      if (bounds.x_min >= bounds.x_max || bounds.y_min >= bounds.y_max) return {x_min: 0, x_max: 5, y_min: 0, y_max: 5};
+      return bounds;
+    }
+
+    function mapToCanvas(point, bounds) {
+      const padding = 28;
+      const width = canvas.width - padding * 2;
+      const height = canvas.height - padding * 2;
+      const scale = Math.min(width / (bounds.x_max - bounds.x_min), height / (bounds.y_max - bounds.y_min));
+      const drawWidth = (bounds.x_max - bounds.x_min) * scale;
+      const drawHeight = (bounds.y_max - bounds.y_min) * scale;
+      const offsetX = (canvas.width - drawWidth) / 2;
+      const offsetY = (canvas.height - drawHeight) / 2;
+      return {
+        x: offsetX + (point.x - bounds.x_min) * scale,
+        y: canvas.height - offsetY - (point.y - bounds.y_min) * scale
+      };
+    }
+
+    function drawMapImage(bounds) {
+      if (!showMap.checked || !mapImage.complete || mapImage.naturalWidth === 0) return;
+      const topLeft = mapToCanvas({x: 0, y: 5}, bounds);
+      const bottomRight = mapToCanvas({x: 5, y: 0}, bounds);
+      ctx.save();
+      ctx.globalAlpha = 0.48;
+      ctx.drawImage(mapImage, topLeft.x, topLeft.y, bottomRight.x - topLeft.x, bottomRight.y - topLeft.y);
+      ctx.restore();
+    }
+
+    function wallLine(name, bounds) {
+      if (name === 'left') return [{x: bounds.x_min, y: bounds.y_min}, {x: bounds.x_min, y: bounds.y_max}];
+      if (name === 'right') return [{x: bounds.x_max, y: bounds.y_min}, {x: bounds.x_max, y: bounds.y_max}];
+      if (name === 'bottom') return [{x: bounds.x_min, y: bounds.y_min}, {x: bounds.x_max, y: bounds.y_min}];
+      return [{x: bounds.x_min, y: bounds.y_max}, {x: bounds.x_max, y: bounds.y_max}];
+    }
+
+    function drawWalls(bounds) {
+      ['top', 'bottom', 'left', 'right'].forEach(name => {
+        const wall = wallByName(name);
+        const color = wall && wall.color ? wall.color : '#64748b';
+        const [start, end] = wallLine(name, bounds);
+        const a = mapToCanvas(start, bounds);
+        const b = mapToCanvas(end, bounds);
+        ctx.strokeStyle = color;
+        ctx.lineWidth = 4;
+        ctx.beginPath();
+        ctx.moveTo(a.x, a.y);
+        ctx.lineTo(b.x, b.y);
+        ctx.stroke();
+      });
+    }
+
+    function drawPose(pose, color, radius) {
+      if (!pose || !Number.isFinite(Number(pose.x)) || !Number.isFinite(Number(pose.y))) return;
+      const bounds = computeBounds();
+      const p = mapToCanvas({x: Number(pose.x), y: Number(pose.y)}, bounds);
+      ctx.fillStyle = color;
+      ctx.beginPath();
+      ctx.arc(p.x, p.y, radius, 0, Math.PI * 2);
+      ctx.fill();
+      if (Number.isFinite(Number(pose.theta))) {
+        ctx.strokeStyle = color;
+        ctx.lineWidth = 2;
+        ctx.beginPath();
+        ctx.moveTo(p.x, p.y);
+        ctx.lineTo(p.x + Math.cos(Number(pose.theta)) * radius * 2.4, p.y - Math.sin(Number(pose.theta)) * radius * 2.4);
+        ctx.stroke();
+      }
+    }
+
+    function drawScan(bounds) {
+      if (!showScan.checked || !state.scan_map || !Array.isArray(state.scan_map.points)) return;
+      ctx.fillStyle = getComputedStyle(document.documentElement).getPropertyValue('--scan').trim();
+      state.scan_map.points.forEach(point => {
+        const p = mapToCanvas({x: Number(point.x), y: Number(point.y)}, bounds);
+        ctx.beginPath();
+        ctx.arc(p.x, p.y, 2, 0, Math.PI * 2);
+        ctx.fill();
+      });
+    }
+
+    function poseText(pose) {
+      if (!pose) return '-';
+      const theta = Number.isFinite(Number(pose.theta)) ? `${(Number(pose.theta) * 180 / Math.PI).toFixed(1)} deg` : '-';
+      return `x ${Number(pose.x).toFixed(3)}, y ${Number(pose.y).toFixed(3)}, yaw ${theta}`;
+    }
+
+    function render() {
+      const bounds = computeBounds();
+      ctx.clearRect(0, 0, canvas.width, canvas.height);
+      drawMapImage(bounds);
+      drawScan(bounds);
+      drawWalls(bounds);
+      drawPose(state.odom_combined, getComputedStyle(document.documentElement).getPropertyValue('--wall-pose').trim(), 7);
+      combinedPoseEl.textContent = poseText(state.odom_combined);
+      boundsTextEl.textContent = `x [${bounds.x_min.toFixed(2)}, ${bounds.x_max.toFixed(2)}], y [${bounds.y_min.toFixed(2)}, ${bounds.y_max.toFixed(2)}]`;
+    }
+
+    async function loadWallFit() {
+      const response = await fetch('/wall_fit.json', {cache: 'no-store'});
+      state.wall_fit = await response.json();
+      render();
+    }
+
+    function connectEvents() {
+      const events = new EventSource('/events');
+      events.addEventListener('snapshot', event => {
+        const data = JSON.parse(event.data);
+        state = {...state, ...data};
+        statusEl.textContent = data.status || 'ok';
+        render();
+      });
+      events.onerror = () => {
+        statusEl.textContent = 'reconnecting';
+      };
+    }
+
+    showMap.addEventListener('change', render);
+    showScan.addEventListener('change', render);
+    mapImage.addEventListener('load', render);
+    loadWallFit().then(connectEvents).catch(error => {
+      statusEl.textContent = String(error);
+      connectEvents();
+    });
+  </script>
+</body>
+</html>

src/origincar_base/src/origincar_base.cpp

@@ -41,6 +41,24 @@ namespace
     return std::fabs(vx) < kStationaryLinearSpeedThreshold &&
            std::fabs(vy) < kStationaryLinearSpeedThreshold;
   }
+
+  std::vector<::origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride)
+  {
+    std::vector<::origincar_wall::WallPoint> points;
+    stride = std::max(1, stride);
+    points.reserve(msg.ranges.size() / static_cast<std::size_t>(stride) + 1);
+    for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast<std::size_t>(stride))
+    {
+      const auto distance = static_cast<double>(msg.ranges[index]);
+      if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
+      {
+        continue;
+      }
+      const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
+      points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
+    }
+    return points;
+  }
 }
 
 int main(int argc, char *argv[])
@@ -150,7 +168,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr
   if (sign_switch->data == -1)
   {
     memset(&Robot_Pos, 0, sizeof(Robot_Pos));
-    memset(&Mpu6050, 0, sizeof(Mpu6050));
+    Mpu6050 = sensor_msgs::msg::Imu();
     Robot_Pos.X = 0.54;
     Robot_Pos.Y = 0.2;
     Robot_Pos.Z = 0;
@@ -225,9 +243,10 @@ void origincar_base::Publish_Odom()
 
   odom.pose.pose.position.z = 0.0;
   odom.pose.pose.orientation = odom_quat;
-  odom.pose.covariance[0] = odom_pose_cov_x_;
+  const auto covariance = wall_filter_ ? wall_filter_->covariance() : ::origincar_wall::WallKalmanNoise{odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_};
-  odom.pose.covariance[7] = odom_pose_cov_y_;
+  odom.pose.covariance[0] = covariance.x;
-  odom.pose.covariance[35] = odom_pose_cov_yaw_;
+  odom.pose.covariance[7] = covariance.y;
+  odom.pose.covariance[35] = covariance.theta;
   // odom_quat;
   // odom_quat;
 
@@ -263,6 +282,49 @@ void origincar_base::Publish_Odom()
   robotvel_publisher->publish(robotvel);
 }
 
+void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
+{
+  const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
+  const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
+  std::lock_guard<std::mutex> lock(wall_scan_mutex_);
+  latest_scan_points_ = pose_frame_points;
+  has_latest_scan_ = true;
+  latest_scan_consumed_ = false;
+}
+
+void origincar_base::Apply_Wall_Update()
+{
+  if (!wall_filter_)
+  {
+    return;
+  }
+  std::vector<::origincar_wall::WallPoint> scan_points;
+  {
+    std::lock_guard<std::mutex> lock(wall_scan_mutex_);
+    if (!has_latest_scan_ || latest_scan_consumed_)
+    {
+      return;
+    }
+    scan_points = latest_scan_points_;
+    latest_scan_consumed_ = true;
+  }
+
+  const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
+  if (!result.ok)
+  {
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
+                         "wall scan update skipped: %s", result.message.c_str());
+    return;
+  }
+
+  ::origincar_wall::WallKalmanNoise noise{0.015, 0.015, 0.025};
+  if (result.quality.wall_count < 3 || (result.quality.has_mean_error && result.quality.mean_error > 0.18))
+  {
+    noise = {0.08, 0.08, 0.12};
+  }
+  wall_filter_->correct(result.pose, noise);
+}
+
 void origincar_base::Publish_Voltage()
 {
   std_msgs::msg::Float32 voltage_msgs;
@@ -448,14 +510,20 @@ void origincar_base::Control()
     Sampling_Time = (current_time - last_time).seconds();
     if (true == Get_Sensor_Data())
     {
-      Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
+      rclcpp::spin_some(this->get_node_base_interface());
-      Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125
+      if (wall_filter_)
-      Robot_Pos.Z += Robot_Vel.Z * Sampling_Time;
+      {
+        wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time);
+        Apply_Wall_Update();
+        const auto fused_pose = wall_filter_->pose();
+        Robot_Pos.X = static_cast<float>(fused_pose.x);
+        Robot_Pos.Y = static_cast<float>(fused_pose.y);
+        Robot_Pos.Z = static_cast<float>(fused_pose.theta);
+      }
       Publish_ImuSensor();
       Publish_GyroDebug();
       Publish_Voltage();
       Publish_Odom();
-      rclcpp::spin_some(this->get_node_base_interface());
     }
     last_time = current_time;
   }
@@ -480,18 +548,26 @@ origincar_base::origincar_base()
   gyro_z_median_count_ = 0;
   gyro_z_low_pass_ = 0.0f;
   gyro_z_low_pass_initialized_ = false;
-  Robot_Pos.X = 0.54;
+  has_latest_scan_ = false;
-  Robot_Pos.Y = 0.2;
+  latest_scan_consumed_ = true;
 
   int serial_baud_rate = 115200;
 
   this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
+  this->declare_parameter<int>("serial_baud_rate", 115200);
   this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
   this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
   this->declare_parameter<std::string>("odom_frame_id", "odom");
-  this->declare_parameter<std::string>("robot_frame_id", "base_link");
+  this->declare_parameter<std::string>("robot_frame_id", "base_footprint");
   this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
-  this->declare_parameter<bool>("publish_tf", false);
+  this->declare_parameter<std::string>("combined_odom_topic", "odom_combined");
+  this->declare_parameter<std::string>("scan_topic", "/scan");
+  this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
+  this->declare_parameter<bool>("publish_tf", true);
+  this->declare_parameter<int>("wall_scan_stride", 2);
+  this->declare_parameter<double>("laser_x", 0.0);
+  this->declare_parameter<double>("laser_y", 0.0);
+  this->declare_parameter<double>("laser_yaw", 0.0);
   this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
 
   // Odom covariance parameters (tunable via YAML)
@@ -506,7 +582,16 @@ origincar_base::origincar_base()
   this->get_parameter("odom_frame_id", odom_frame_id);
   this->get_parameter("robot_frame_id", robot_frame_id);
   this->get_parameter("gyro_frame_id", gyro_frame_id);
+  this->get_parameter("combined_odom_topic", combined_odom_topic_);
+  this->get_parameter("scan_topic", scan_topic_);
+  this->get_parameter("wall_config_path", wall_config_path_);
   this->get_parameter("publish_tf", publish_tf_);
+  this->get_parameter("wall_scan_stride", wall_scan_stride_);
+  laser_pose_ = {
+      this->get_parameter("laser_x").as_double(),
+      this->get_parameter("laser_y").as_double(),
+      this->get_parameter("laser_yaw").as_double(),
+  };
   gyro_z_low_pass_alpha_ =
       static_cast<float>(this->get_parameter("gyro_z_low_pass_alpha").as_double());
   if (gyro_z_low_pass_alpha_ < 0.0f)
@@ -521,7 +606,18 @@ origincar_base::origincar_base()
   this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
   this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
 
-  odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+  wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
+  const auto initial_pose = ::origincar_wall::Pose2D{
+      wall_fit_config_.coordinate_frame.odom_zero_map.x,
+      wall_fit_config_.coordinate_frame.odom_zero_map.y,
+      0.0,
+  };
+  wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
+  Robot_Pos.X = static_cast<float>(initial_pose.x);
+  Robot_Pos.Y = static_cast<float>(initial_pose.y);
+  Robot_Pos.Z = static_cast<float>(initial_pose.theta);
+
+  odom_publisher = create_publisher<nav_msgs::msg::Odometry>(combined_odom_topic_, 10);
 
   imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);
 
@@ -543,12 +639,14 @@ origincar_base::origincar_base()
       cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1));
   Akm_Cmd_Vel_Sub = create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
       akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
+  Scan_Sub = create_subscription<sensor_msgs::msg::LaserScan>(
+      scan_topic_, 10, std::bind(&origincar_base::Scan_Callback, this, _1));
 
   Sign_Switch_Sub = create_subscription<std_msgs::msg::Int32>(
       "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
   try
   {
-    Stm32_Serial.setPort("/dev/ttyACM0");
+    Stm32_Serial.setPort(usart_port_name);
     Stm32_Serial.setBaudrate(serial_baud_rate);
     serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
     Stm32_Serial.setTimeout(_time);
@@ -605,7 +703,7 @@ void sigintHandler(int sig)
     {
     }
   }
-  // 关闭ROS2接口，清除资源
+  // Shutdown ROS2 and release resources.
   rclcpp::shutdown();
 }
 
@@ -613,3 +711,4 @@ origincar_base::~origincar_base()
 {
   RCLCPP_INFO(this->get_logger(), "Shutting down");
 }
+

src/origincar_base/src/wall_fit_calibrator.cpp

@@ -0,0 +1,246 @@
+#include "origincar_base/wall_fit_core.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#include <poll.h>
+#include <termios.h>
+#include <unistd.h>
+
+#include <atomic>
+#include <cmath>
+#include <map>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace
+{
+std::vector<origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg)
+{
+  std::vector<origincar_wall::WallPoint> points;
+  points.reserve(msg.ranges.size());
+  for (std::size_t index = 0; index < msg.ranges.size(); ++index)
+  {
+    const auto distance = static_cast<double>(msg.ranges[index]);
+    if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
+    {
+      continue;
+    }
+    const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
+    points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
+  }
+  return points;
+}
+
+origincar_wall::Pose2D odomMsgToMapPose(const nav_msgs::msg::Odometry &msg,
+                                        const origincar_wall::WallFitConfig &config)
+{
+  const auto &q = msg.pose.pose.orientation;
+  const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
+  const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
+  return {
+      msg.pose.pose.position.x + config.coordinate_frame.odom_zero_map.x,
+      msg.pose.pose.position.y + config.coordinate_frame.odom_zero_map.y,
+      std::atan2(siny_cosp, cosy_cosp),
+  };
+}
+}  // namespace
+
+class WallFitCalibrator : public rclcpp::Node
+{
+public:
+  WallFitCalibrator()
+      : Node("wall_fit_calibrator")
+  {
+    declare_parameter<std::string>("scan_topic", "/scan");
+    declare_parameter<std::string>("odom_topic", "/odom_combined");
+    declare_parameter<std::string>("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
+    declare_parameter<double>("laser_x", 0.0);
+    declare_parameter<double>("laser_y", 0.0);
+    declare_parameter<double>("laser_yaw", 0.0);
+
+    config_path_ = get_parameter("config_path").as_string();
+    config_ = origincar_wall::loadWallFitConfig(config_path_);
+    laser_pose_ = {
+        get_parameter("laser_x").as_double(),
+        get_parameter("laser_y").as_double(),
+        get_parameter("laser_yaw").as_double(),
+    };
+
+    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+        get_parameter("scan_topic").as_string(), 10,
+        [this](sensor_msgs::msg::LaserScan::SharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          latest_scan_ = origincar_wall::transformScanPointsToPoseFrame(scanMsgToPoints(*msg), laser_pose_);
+          has_scan_ = true;
+        });
+    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        get_parameter("odom_topic").as_string(), 10,
+        [this](nav_msgs::msg::Odometry::SharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          latest_odom_ = odomMsgToMapPose(*msg, config_);
+          has_odom_ = true;
+        });
+
+    printHelp();
+    keyboard_thread_ = std::thread([this]() { keyboardLoop(); });
+  }
+
+  ~WallFitCalibrator() override
+  {
+    running_ = false;
+    if (keyboard_thread_.joinable())
+    {
+      keyboard_thread_.join();
+    }
+  }
+
+private:
+  void printHelp()
+  {
+    RCLCPP_INFO(get_logger(), "Wall calibration order: P -> task_release -> passage -> clinic");
+    RCLCPP_INFO(get_logger(), "Keys: Space=capture, A=previous, D=next, Q=clear, Enter=save");
+    printSelected();
+  }
+
+  void printSelected()
+  {
+    RCLCPP_INFO(get_logger(), "Selected [%zu/%zu]: %s",
+                selected_index_ + 1, reference_order_.size(), reference_order_[selected_index_].c_str());
+  }
+
+  void captureCurrent()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!has_scan_ || !has_odom_)
+    {
+      RCLCPP_WARN(get_logger(), "Waiting for both /scan and /odom_combined before capture");
+      return;
+    }
+    const auto &key = reference_order_[selected_index_];
+    const auto reference = config_.reference_points.at(key);
+    origincar_wall::WallFitCapture capture;
+    capture.key = key;
+    capture.map = reference.map;
+    capture.odom_pose = {reference.map.x, reference.map.y, latest_odom_.theta};
+    capture.scan_points = latest_scan_;
+    captures_[key] = capture;
+    RCLCPP_INFO(get_logger(), "Captured %s with %zu scan points", key.c_str(), latest_scan_.size());
+  }
+
+  void clearCaptures()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    captures_.clear();
+    RCLCPP_WARN(get_logger(), "Cleared all wall calibration captures");
+  }
+
+  void saveCaptures()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<origincar_wall::WallFitCapture> ordered_captures;
+    for (const auto &key : reference_order_)
+    {
+      const auto found = captures_.find(key);
+      if (found == captures_.end())
+      {
+        RCLCPP_WARN(get_logger(), "Missing capture for %s; not saving", key.c_str());
+        return;
+      }
+      ordered_captures.push_back(found->second);
+    }
+    try
+    {
+      const auto fitted = origincar_wall::fitWallConfigFromCaptures(ordered_captures, config_);
+      origincar_wall::saveWallFitConfig(config_path_, fitted);
+      config_ = fitted;
+      const auto bounds = origincar_wall::wallBounds(config_);
+      RCLCPP_INFO(get_logger(), "Saved %s; bounds x=[%.3f, %.3f] y=[%.3f, %.3f]",
+                  config_path_.c_str(), bounds.x_min, bounds.x_max, bounds.y_min, bounds.y_max);
+    }
+    catch (const std::exception &error)
+    {
+      RCLCPP_ERROR(get_logger(), "Failed to save wall fit: %s", error.what());
+    }
+  }
+
+  void keyboardLoop()
+  {
+    termios old_terminal{};
+    if (tcgetattr(STDIN_FILENO, &old_terminal) != 0)
+    {
+      RCLCPP_WARN(get_logger(), "Keyboard control unavailable: stdin is not a terminal");
+      return;
+    }
+    termios raw_terminal = old_terminal;
+    raw_terminal.c_lflag &= static_cast<unsigned int>(~(ICANON | ECHO));
+    raw_terminal.c_cc[VMIN] = 0;
+    raw_terminal.c_cc[VTIME] = 0;
+    tcsetattr(STDIN_FILENO, TCSANOW, &raw_terminal);
+
+    while (running_ && rclcpp::ok())
+    {
+      pollfd descriptor{STDIN_FILENO, POLLIN, 0};
+      const auto ready = poll(&descriptor, 1, 100);
+      if (ready <= 0 || !(descriptor.revents & POLLIN))
+      {
+        continue;
+      }
+      char key = 0;
+      if (read(STDIN_FILENO, &key, 1) != 1)
+      {
+        continue;
+      }
+      if (key == ' ')
+      {
+        captureCurrent();
+      }
+      else if (key == 'q' || key == 'Q')
+      {
+        clearCaptures();
+      }
+      else if (key == 'a' || key == 'A')
+      {
+        selected_index_ = selected_index_ == 0 ? reference_order_.size() - 1 : selected_index_ - 1;
+        printSelected();
+      }
+      else if (key == 'd' || key == 'D')
+      {
+        selected_index_ = (selected_index_ + 1) % reference_order_.size();
+        printSelected();
+      }
+      else if (key == '\n' || key == '\r')
+      {
+        saveCaptures();
+      }
+    }
+    tcsetattr(STDIN_FILENO, TCSANOW, &old_terminal);
+  }
+
+  std::string config_path_;
+  origincar_wall::WallFitConfig config_;
+  origincar_wall::Pose2D laser_pose_{0.0, 0.0, 0.0};
+  std::vector<std::string> reference_order_{"P", "task_release", "passage", "clinic"};
+  std::size_t selected_index_{0};
+  std::mutex mutex_;
+  bool has_scan_{false};
+  bool has_odom_{false};
+  std::vector<origincar_wall::WallPoint> latest_scan_;
+  origincar_wall::Pose2D latest_odom_{0.0, 0.0, 0.0};
+  std::map<std::string, origincar_wall::WallFitCapture> captures_;
+  std::atomic<bool> running_{true};
+  std::thread keyboard_thread_;
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<WallFitCalibrator>());
+  rclcpp::shutdown();
+  return 0;
+}

src/origincar_base/src/wall_fit_core.cpp

@@ -0,0 +1,605 @@
+#include "origincar_base/wall_fit_core.hpp"
+
+#include <nlohmann/json.hpp>
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <fstream>
+#include <iomanip>
+#include <limits>
+#include <sstream>
+#include <stdexcept>
+#include <unistd.h>
+
+namespace origincar_wall
+{
+namespace
+{
+constexpr double kPi = 3.14159265358979323846;
+constexpr double kYawSearchRange = 8.0 * kPi / 180.0;
+constexpr double kYawSearchStep = 1.0 * kPi / 180.0;
+constexpr double kWallGroupMaxDistance = 0.45;
+constexpr int kMinScanPoints = 8;
+constexpr int kMinWallPoints = 3;
+
+double mean(const std::vector<double> &values)
+{
+  if (values.empty())
+  {
+    throw std::runtime_error("mean requires values");
+  }
+  double total = 0.0;
+  for (const auto value : values)
+  {
+    total += value;
+  }
+  return total / static_cast<double>(values.size());
+}
+
+double median(std::vector<double> values)
+{
+  if (values.empty())
+  {
+    throw std::runtime_error("median requires values");
+  }
+  std::sort(values.begin(), values.end());
+  const auto middle = values.size() / 2;
+  if (values.size() % 2 == 1)
+  {
+    return values[middle];
+  }
+  return (values[middle - 1] + values[middle]) / 2.0;
+}
+
+double percentile(std::vector<double> values, double fraction)
+{
+  if (values.empty())
+  {
+    throw std::runtime_error("percentile requires values");
+  }
+  std::sort(values.begin(), values.end());
+  const auto raw_index = static_cast<int>(std::round((values.size() - 1) * fraction));
+  const auto index = std::max(0, std::min(raw_index, static_cast<int>(values.size() - 1)));
+  return values[static_cast<std::size_t>(index)];
+}
+
+WallLine wallByName(const WallFitConfig &config, const std::string &name)
+{
+  for (const auto &wall : config.walls)
+  {
+    if (wall.name == name)
+    {
+      return wall;
+    }
+  }
+  throw std::runtime_error("missing wall " + name);
+}
+
+double verticalWallX(const WallLine &wall)
+{
+  if (std::fabs(wall.a) < 1e-9 || std::fabs(wall.b) > 1e-6)
+  {
+    throw std::runtime_error("wall " + wall.name + " must be vertical");
+  }
+  return -wall.c / wall.a;
+}
+
+double horizontalWallY(const WallLine &wall)
+{
+  if (std::fabs(wall.b) < 1e-9 || std::fabs(wall.a) > 1e-6)
+  {
+    throw std::runtime_error("wall " + wall.name + " must be horizontal");
+  }
+  return -wall.c / wall.b;
+}
+
+std::pair<std::string, double> nearestWallResidual(const WallPoint &point, const WallBounds &bounds)
+{
+  std::array<std::pair<std::string, double>, 4> candidates{{
+      {"left", point.x - bounds.x_min},
+      {"right", point.x - bounds.x_max},
+      {"bottom", point.y - bounds.y_min},
+      {"top", point.y - bounds.y_max},
+  }};
+  return *std::min_element(
+      candidates.begin(), candidates.end(),
+      [](const auto &left, const auto &right) { return std::fabs(left.second) < std::fabs(right.second); });
+}
+
+std::map<std::string, std::vector<WallPoint>> groupScanPointsByWall(
+    const std::vector<WallPoint> &scan_map, const WallBounds &bounds)
+{
+  std::map<std::string, std::vector<WallPoint>> groups{
+      {"left", {}}, {"right", {}}, {"bottom", {}}, {"top", {}}};
+  for (const auto &point : scan_map)
+  {
+    const auto nearest = nearestWallResidual(point, bounds);
+    if (std::fabs(nearest.second) <= kWallGroupMaxDistance)
+    {
+      groups[nearest.first].push_back(point);
+    }
+  }
+  return groups;
+}
+
+std::vector<double> yawCandidates(double center_theta)
+{
+  std::vector<double> candidates;
+  const auto steps = static_cast<int>(std::round(kYawSearchRange / kYawSearchStep));
+  for (int index = -steps; index <= steps; ++index)
+  {
+    candidates.push_back(normalizeAngle(center_theta + index * kYawSearchStep));
+  }
+  return candidates;
+}
+
+LocalizationResult solveWallOffsetsForPose(const std::vector<WallPoint> &scan_points,
+                                           const WallBounds &bounds,
+                                           const Pose2D &pose)
+{
+  const auto scan_map = transformScanPointsToMap(scan_points, pose);
+  const auto groups = groupScanPointsByWall(scan_map, bounds);
+  std::vector<double> x_corrections;
+  std::vector<double> y_corrections;
+  std::vector<MatchedWall> matched_walls;
+
+  for (const auto &entry : groups)
+  {
+    const auto &name = entry.first;
+    const auto &group_points = entry.second;
+    if (static_cast<int>(group_points.size()) < kMinWallPoints)
+    {
+      continue;
+    }
+    const bool x_axis = (name == "left" || name == "right");
+    const double target = name == "left"    ? bounds.x_min
+                          : name == "right" ? bounds.x_max
+                          : name == "bottom" ? bounds.y_min
+                                             : bounds.y_max;
+    std::vector<double> observed_values;
+    observed_values.reserve(group_points.size());
+    for (const auto &point : group_points)
+    {
+      observed_values.push_back(x_axis ? point.x : point.y);
+    }
+    const auto observed = median(observed_values);
+    const auto correction = target - observed;
+    std::vector<double> errors;
+    errors.reserve(group_points.size());
+    for (const auto &value : observed_values)
+    {
+      errors.push_back(std::fabs((value + correction) - target));
+    }
+    matched_walls.push_back(
+        {name, static_cast<int>(group_points.size()), observed, target, correction, mean(errors)});
+    if (x_axis)
+    {
+      x_corrections.push_back(correction);
+    }
+    else
+    {
+      y_corrections.push_back(correction);
+    }
+  }
+
+  LocalizationQuality quality;
+  quality.wall_count = static_cast<int>(matched_walls.size());
+  quality.has_mean_error = !matched_walls.empty();
+  quality.mean_error = 0.0;
+  if (quality.has_mean_error)
+  {
+    std::vector<double> wall_errors;
+    for (const auto &wall : matched_walls)
+    {
+      wall_errors.push_back(wall.mean_error);
+    }
+    quality.mean_error = mean(wall_errors);
+  }
+
+  const bool valid = matched_walls.size() >= 2 && !x_corrections.empty() && !y_corrections.empty();
+  if (!valid)
+  {
+    return {false, pose, scan_map, matched_walls, quality, "fewer than two usable perpendicular wall constraints"};
+  }
+
+  Pose2D corrected_pose{
+      pose.x + mean(x_corrections),
+      pose.y + mean(y_corrections),
+      normalizeAngle(pose.theta),
+  };
+  return {
+      true,
+      corrected_pose,
+      transformScanPointsToMap(scan_points, corrected_pose),
+      matched_walls,
+      quality,
+      "localized from odom prediction and calibrated walls"};
+}
+
+nlohmann::json pointToJson(const WallPoint &point)
+{
+  return nlohmann::json{{"x", point.x}, {"y", point.y}};
+}
+
+WallPoint pointFromJson(const nlohmann::json &json)
+{
+  return {json.at("x").get<double>(), json.at("y").get<double>()};
+}
+
+nlohmann::json wallToJson(const WallLine &wall)
+{
+  return nlohmann::json{{"name", wall.name}, {"a", wall.a}, {"b", wall.b}, {"c", wall.c}, {"color", wall.color}};
+}
+
+WallLine wallFromJson(const nlohmann::json &json)
+{
+  return {
+      json.at("name").get<std::string>(),
+      json.at("a").get<double>(),
+      json.at("b").get<double>(),
+      json.at("c").get<double>(),
+      json.value("color", std::string("#64748b"))};
+}
+
+nlohmann::json configToJsonObject(const WallFitConfig &config)
+{
+  nlohmann::json walls = nlohmann::json::array();
+  for (const auto &wall : config.walls)
+  {
+    walls.push_back(wallToJson(wall));
+  }
+  nlohmann::json refs = nlohmann::json::object();
+  for (const auto &entry : config.reference_points)
+  {
+    refs[entry.first] = {
+        {"label", entry.second.label},
+        {"image", pointToJson(entry.second.image)},
+        {"map", pointToJson(entry.second.map)}};
+  }
+  nlohmann::json frame = {
+      {"origin", config.coordinate_frame.origin},
+      {"origin_note", config.coordinate_frame.origin_note},
+      {"width_m", config.coordinate_frame.width_m},
+      {"height_m", config.coordinate_frame.height_m},
+      {"image_width_px", config.coordinate_frame.image_width_px},
+      {"image_height_px", config.coordinate_frame.image_height_px},
+      {"pixels_per_meter", config.coordinate_frame.pixels_per_meter},
+      {"p_image_px", pointToJson(config.coordinate_frame.p_image_px)},
+      {"odom_zero_reference", config.coordinate_frame.odom_zero_reference},
+      {"odom_zero_map", pointToJson(config.coordinate_frame.odom_zero_map)}};
+
+  nlohmann::json result = {{"coordinate_frame", frame}, {"walls", walls}, {"reference_points", refs}};
+  if (!config.generated_at.empty())
+  {
+    result["generated_at"] = config.generated_at;
+  }
+  return result;
+}
+
+WallFitConfig configFromJsonObject(const nlohmann::json &json)
+{
+  auto config = defaultWallFitConfig();
+  const auto &frame = json.at("coordinate_frame");
+  config.coordinate_frame.origin = frame.at("origin").get<std::string>();
+  config.coordinate_frame.origin_note = frame.value("origin_note", config.coordinate_frame.origin_note);
+  config.coordinate_frame.width_m = frame.value("width_m", config.coordinate_frame.width_m);
+  config.coordinate_frame.height_m = frame.value("height_m", config.coordinate_frame.height_m);
+  config.coordinate_frame.image_width_px = frame.value("image_width_px", config.coordinate_frame.image_width_px);
+  config.coordinate_frame.image_height_px = frame.value("image_height_px", config.coordinate_frame.image_height_px);
+  config.coordinate_frame.pixels_per_meter = frame.value("pixels_per_meter", config.coordinate_frame.pixels_per_meter);
+  if (frame.contains("p_image_px"))
+  {
+    config.coordinate_frame.p_image_px = pointFromJson(frame.at("p_image_px"));
+  }
+  config.coordinate_frame.odom_zero_reference =
+      frame.value("odom_zero_reference", config.coordinate_frame.odom_zero_reference);
+  if (frame.contains("odom_zero_map"))
+  {
+    config.coordinate_frame.odom_zero_map = pointFromJson(frame.at("odom_zero_map"));
+  }
+
+  config.walls.clear();
+  for (const auto &wall_json : json.at("walls"))
+  {
+    config.walls.push_back(wallFromJson(wall_json));
+  }
+
+  if (json.contains("reference_points"))
+  {
+    config.reference_points.clear();
+    for (auto it = json.at("reference_points").begin(); it != json.at("reference_points").end(); ++it)
+    {
+      config.reference_points[it.key()] = {
+          it.value().value("label", it.key()),
+          pointFromJson(it.value().at("image")),
+          pointFromJson(it.value().at("map"))};
+    }
+  }
+  config.generated_at = json.value("generated_at", std::string());
+  if (config.coordinate_frame.origin != "map_lower_left")
+  {
+    throw std::runtime_error("wall_fit coordinate_frame.origin must be map_lower_left");
+  }
+  wallBounds(config);
+  return config;
+}
+
+std::string utcNow()
+{
+  const auto now = std::chrono::system_clock::now();
+  const auto time = std::chrono::system_clock::to_time_t(now);
+  std::tm tm{};
+  gmtime_r(&time, &tm);
+  std::ostringstream stream;
+  stream << std::put_time(&tm, "%Y-%m-%dT%H:%M:%SZ");
+  return stream.str();
+}
+}  // namespace
+
+WallFitConfig defaultWallFitConfig()
+{
+  WallFitConfig config;
+  config.coordinate_frame = {
+      "map_lower_left",
+      "map (0,0) is the lower-left map corner; odom zero is placed at the P point",
+      5.0,
+      5.0,
+      500,
+      500,
+      100,
+      {56.0, 482.0},
+      "P",
+      {0.56, 0.18},
+  };
+  config.walls = {
+      {"top", 0.0, -1.0, 5.0, "#d62728"},
+      {"bottom", 0.0, 1.0, 0.0, "#1f77b4"},
+      {"left", 1.0, 0.0, 0.0, "#2ca02c"},
+      {"right", -1.0, 0.0, 5.0, "#ff7f0e"},
+  };
+  config.reference_points = {
+      {"P", {"P", {56.0, 482.0}, {0.56, 0.18}}},
+      {"task_release", {"task_release_left_midpoint", {450.0, 319.0}, {4.5, 1.81}}},
+      {"passage", {"passage", {250.0, 240.0}, {2.5, 2.6}}},
+      {"clinic", {"clinic_room", {280.0, 123.0}, {2.8, 3.77}}},
+  };
+  return config;
+}
+
+WallBounds wallBounds(const WallFitConfig &config)
+{
+  const WallBounds bounds{
+      verticalWallX(wallByName(config, "left")),
+      verticalWallX(wallByName(config, "right")),
+      horizontalWallY(wallByName(config, "bottom")),
+      horizontalWallY(wallByName(config, "top")),
+  };
+  if (bounds.x_min >= bounds.x_max || bounds.y_min >= bounds.y_max)
+  {
+    throw std::runtime_error("wall bounds are invalid");
+  }
+  return bounds;
+}
+
+double normalizeAngle(double angle)
+{
+  return std::fmod(angle + kPi, 2.0 * kPi) < 0.0
+             ? std::fmod(angle + kPi, 2.0 * kPi) + kPi
+             : std::fmod(angle + kPi, 2.0 * kPi) - kPi;
+}
+
+Pose2D predictPoseFromOdomDelta(const Pose2D &previous_wall_pose,
+                                const Pose2D &previous_odom_pose,
+                                const Pose2D &current_odom_pose)
+{
+  return {
+      previous_wall_pose.x + current_odom_pose.x - previous_odom_pose.x,
+      previous_wall_pose.y + current_odom_pose.y - previous_odom_pose.y,
+      normalizeAngle(previous_wall_pose.theta + normalizeAngle(current_odom_pose.theta - previous_odom_pose.theta)),
+  };
+}
+
+std::vector<WallPoint> transformScanPointsToMap(const std::vector<WallPoint> &scan_points,
+                                                const Pose2D &pose)
+{
+  const auto cos_t = std::cos(pose.theta);
+  const auto sin_t = std::sin(pose.theta);
+  std::vector<WallPoint> result;
+  result.reserve(scan_points.size());
+  for (const auto &point : scan_points)
+  {
+    result.push_back({pose.x + point.x * cos_t - point.y * sin_t,
+                      pose.y + point.x * sin_t + point.y * cos_t});
+  }
+  return result;
+}
+
+std::vector<WallPoint> transformScanPointsToPoseFrame(const std::vector<WallPoint> &scan_points,
+                                                      const Pose2D &laser_pose)
+{
+  return transformScanPointsToMap(scan_points, laser_pose);
+}
+
+std::vector<WallPoint> mapPointsToScan(const std::vector<WallPoint> &map_points,
+                                       const Pose2D &pose)
+{
+  const auto cos_t = std::cos(pose.theta);
+  const auto sin_t = std::sin(pose.theta);
+  std::vector<WallPoint> result;
+  result.reserve(map_points.size());
+  for (const auto &point : map_points)
+  {
+    const auto dx = point.x - pose.x;
+    const auto dy = point.y - pose.y;
+    result.push_back({dx * cos_t + dy * sin_t, -dx * sin_t + dy * cos_t});
+  }
+  return result;
+}
+
+LocalizationResult localizeFromScan(const std::vector<WallPoint> &scan_points,
+                                    const WallFitConfig &config,
+                                    const Pose2D &predicted_pose)
+{
+  if (static_cast<int>(scan_points.size()) < kMinScanPoints)
+  {
+    return {
+        false,
+        predicted_pose,
+        transformScanPointsToMap(scan_points, predicted_pose),
+        {},
+        {0, 0.0, false},
+        "not enough scan points for wall localization"};
+  }
+  const auto bounds = wallBounds(config);
+  const auto center_theta = predicted_pose.theta;
+  std::vector<LocalizationResult> valid_results;
+  for (const auto theta : yawCandidates(center_theta))
+  {
+    auto solution = solveWallOffsetsForPose(scan_points, bounds, {predicted_pose.x, predicted_pose.y, theta});
+    if (solution.ok)
+    {
+      valid_results.push_back(solution);
+    }
+  }
+  if (valid_results.empty())
+  {
+    return solveWallOffsetsForPose(scan_points, bounds, predicted_pose);
+  }
+  return *std::min_element(
+      valid_results.begin(), valid_results.end(),
+      [center_theta](const auto &left, const auto &right) {
+        if (std::fabs(left.quality.mean_error - right.quality.mean_error) > 1e-9)
+        {
+          return left.quality.mean_error < right.quality.mean_error;
+        }
+        if (left.quality.wall_count != right.quality.wall_count)
+        {
+          return left.quality.wall_count > right.quality.wall_count;
+        }
+        return std::fabs(normalizeAngle(left.pose.theta - center_theta)) <
+               std::fabs(normalizeAngle(right.pose.theta - center_theta));
+      });
+}
+
+WallFitConfig fitWallConfigFromCaptures(const std::vector<WallFitCapture> &captures,
+                                        const WallFitConfig &base_config)
+{
+  if (captures.size() < 4)
+  {
+    throw std::runtime_error("wall calibration requires four captures");
+  }
+  std::vector<WallPoint> all_points;
+  for (const auto &capture : captures)
+  {
+    const auto scan_map = transformScanPointsToMap(capture.scan_points, capture.odom_pose);
+    all_points.insert(all_points.end(), scan_map.begin(), scan_map.end());
+  }
+  if (all_points.size() < 16)
+  {
+    throw std::runtime_error("wall calibration requires enough scan points");
+  }
+  std::vector<double> xs;
+  std::vector<double> ys;
+  xs.reserve(all_points.size());
+  ys.reserve(all_points.size());
+  for (const auto &point : all_points)
+  {
+    if (std::isfinite(point.x))
+    {
+      xs.push_back(point.x);
+    }
+    if (std::isfinite(point.y))
+    {
+      ys.push_back(point.y);
+    }
+  }
+  const auto left_cut = percentile(xs, 0.08);
+  const auto right_cut = percentile(xs, 0.92);
+  const auto bottom_cut = percentile(ys, 0.08);
+  const auto top_cut = percentile(ys, 0.92);
+  std::vector<double> left_values;
+  std::vector<double> right_values;
+  std::vector<double> bottom_values;
+  std::vector<double> top_values;
+  for (const auto value : xs)
+  {
+    if (value <= left_cut)
+    {
+      left_values.push_back(value);
+    }
+    if (value >= right_cut)
+    {
+      right_values.push_back(value);
+    }
+  }
+  for (const auto value : ys)
+  {
+    if (value <= bottom_cut)
+    {
+      bottom_values.push_back(value);
+    }
+    if (value >= top_cut)
+    {
+      top_values.push_back(value);
+    }
+  }
+
+  const auto x_min = median(left_values);
+  const auto x_max = median(right_values);
+  const auto y_min = median(bottom_values);
+  const auto y_max = median(top_values);
+
+  auto config = base_config;
+  config.coordinate_frame = defaultWallFitConfig().coordinate_frame;
+  config.reference_points = defaultWallFitConfig().reference_points;
+  config.walls = {
+      {"top", 0.0, -1.0, y_max, "#d62728"},
+      {"bottom", 0.0, 1.0, -y_min, "#1f77b4"},
+      {"left", 1.0, 0.0, -x_min, "#2ca02c"},
+      {"right", -1.0, 0.0, x_max, "#ff7f0e"},
+  };
+  config.generated_at = utcNow();
+  wallBounds(config);
+  return config;
+}
+
+WallFitConfig loadWallFitConfig(const std::string &path)
+{
+  std::ifstream file(path);
+  if (!file.good())
+  {
+    return defaultWallFitConfig();
+  }
+  nlohmann::json json;
+  file >> json;
+  return configFromJsonObject(json);
+}
+
+void saveWallFitConfig(const std::string &path, const WallFitConfig &config)
+{
+  const auto json = configToJsonObject(config);
+  const auto temp_path = path + ".tmp";
+  {
+    std::ofstream file(temp_path, std::ios::trunc);
+    if (!file.good())
+    {
+      throw std::runtime_error("failed to open temporary wall fit config");
+    }
+    file << json.dump(2) << "\n";
+  }
+  if (std::rename(temp_path.c_str(), path.c_str()) != 0)
+  {
+    std::remove(temp_path.c_str());
+    throw std::runtime_error("failed to replace wall fit config");
+  }
+}
+
+std::string wallFitConfigToJson(const WallFitConfig &config)
+{
+  return configToJsonObject(config).dump(2);
+}
+
+}  // namespace origincar_wall

src/origincar_base/src/wall_kalman_filter.cpp

@@ -0,0 +1,66 @@
+#include "origincar_base/wall_kalman_filter.hpp"
+
+#include <algorithm>
+#include <cmath>
+
+namespace origincar_wall
+{
+
+WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
+    : pose_(initial_pose),
+      covariance_{0.10, 0.10, 0.10},
+      process_noise_per_second_{0.04, 0.04, 0.06}
+{
+  pose_.theta = normalizeAngle(pose_.theta);
+}
+
+void WallKalmanFilter::reset(const Pose2D &pose)
+{
+  pose_ = pose;
+  pose_.theta = normalizeAngle(pose_.theta);
+  covariance_ = {0.10, 0.10, 0.10};
+}
+
+void WallKalmanFilter::predict(double vx, double vy, double yaw_rate, double dt)
+{
+  dt = std::max(0.0, dt);
+  const auto cos_t = std::cos(pose_.theta);
+  const auto sin_t = std::sin(pose_.theta);
+  pose_.x += (vx * cos_t - vy * sin_t) * dt;
+  pose_.y += (vx * sin_t + vy * cos_t) * dt;
+  pose_.theta = normalizeAngle(pose_.theta + yaw_rate * dt);
+  covariance_.x += process_noise_per_second_.x * dt;
+  covariance_.y += process_noise_per_second_.y * dt;
+  covariance_.theta += process_noise_per_second_.theta * dt;
+}
+
+void WallKalmanFilter::correct(const Pose2D &measurement, const WallKalmanNoise &measurement_noise)
+{
+  const auto noise_x = std::max(1e-6, measurement_noise.x);
+  const auto noise_y = std::max(1e-6, measurement_noise.y);
+  const auto noise_theta = std::max(1e-6, measurement_noise.theta);
+
+  const auto kx = covariance_.x / (covariance_.x + noise_x);
+  const auto ky = covariance_.y / (covariance_.y + noise_y);
+  const auto kt = covariance_.theta / (covariance_.theta + noise_theta);
+
+  pose_.x += kx * (measurement.x - pose_.x);
+  pose_.y += ky * (measurement.y - pose_.y);
+  pose_.theta = normalizeAngle(pose_.theta + kt * normalizeAngle(measurement.theta - pose_.theta));
+
+  covariance_.x = (1.0 - kx) * covariance_.x;
+  covariance_.y = (1.0 - ky) * covariance_.y;
+  covariance_.theta = (1.0 - kt) * covariance_.theta;
+}
+
+Pose2D WallKalmanFilter::pose() const
+{
+  return pose_;
+}
+
+WallKalmanNoise WallKalmanFilter::covariance() const
+{
+  return covariance_;
+}
+
+}  // namespace origincar_wall

src/origincar_base/src/wall_localization_node.cpp

@@ -0,0 +1,256 @@
+#include "origincar_base/wall_fit_core.hpp"
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <cmath>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace
+{
+std::vector<origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride)
+{
+  std::vector<origincar_wall::WallPoint> points;
+  stride = std::max(1, stride);
+  points.reserve(msg.ranges.size() / static_cast<std::size_t>(stride) + 1);
+  for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast<std::size_t>(stride))
+  {
+    const auto distance = static_cast<double>(msg.ranges[index]);
+    if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
+    {
+      continue;
+    }
+    const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
+    points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
+  }
+  return points;
+}
+
+double quaternionToYaw(const geometry_msgs::msg::Quaternion &q)
+{
+  const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
+  const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
+  return std::atan2(siny_cosp, cosy_cosp);
+}
+
+geometry_msgs::msg::Quaternion yawToQuaternion(double yaw)
+{
+  geometry_msgs::msg::Quaternion q;
+  const auto half = yaw / 2.0;
+  q.x = 0.0;
+  q.y = 0.0;
+  q.z = std::sin(half);
+  q.w = std::cos(half);
+  return q;
+}
+
+origincar_wall::Pose2D odomMsgToMapPose(const nav_msgs::msg::Odometry &msg,
+                                        const origincar_wall::WallFitConfig &config)
+{
+  return {
+      msg.pose.pose.position.x + config.coordinate_frame.odom_zero_map.x,
+      msg.pose.pose.position.y + config.coordinate_frame.odom_zero_map.y,
+      quaternionToYaw(msg.pose.pose.orientation),
+  };
+}
+}  // namespace
+
+class WallLocalizationNode : public rclcpp::Node
+{
+public:
+  WallLocalizationNode()
+      : Node("wall_localization_node"),
+        tf_broadcaster_(std::make_unique<tf2_ros::TransformBroadcaster>(*this))
+  {
+    declare_parameter<std::string>("scan_topic", "/scan");
+    declare_parameter<std::string>("odom_topic", "/odom_combined");
+    declare_parameter<std::string>("wall_odom_topic", "/odom_wall");
+    declare_parameter<std::string>("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
+    declare_parameter<std::string>("odom_frame", "odom");
+    declare_parameter<std::string>("base_frame", "base_footprint");
+    declare_parameter<bool>("publish_tf", true);
+    declare_parameter<double>("localization_hz", 10.0);
+    declare_parameter<int>("scan_stride", 2);
+    declare_parameter<double>("laser_x", 0.0);
+    declare_parameter<double>("laser_y", 0.0);
+    declare_parameter<double>("laser_yaw", 0.0);
+
+    config_ = origincar_wall::loadWallFitConfig(get_parameter("config_path").as_string());
+    wall_odom_topic_ = get_parameter("wall_odom_topic").as_string();
+    odom_frame_ = get_parameter("odom_frame").as_string();
+    base_frame_ = get_parameter("base_frame").as_string();
+    publish_tf_ = get_parameter("publish_tf").as_bool();
+    scan_stride_ = get_parameter("scan_stride").as_int();
+    const auto localization_hz = std::max(1.0, get_parameter("localization_hz").as_double());
+    min_scan_period_ = rclcpp::Duration::from_seconds(1.0 / localization_hz);
+    laser_pose_ = {
+        get_parameter("laser_x").as_double(),
+        get_parameter("laser_y").as_double(),
+        get_parameter("laser_yaw").as_double(),
+    };
+
+    wall_odom_pub_ = create_publisher<nav_msgs::msg::Odometry>(wall_odom_topic_, 10);
+    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+        get_parameter("scan_topic").as_string(), 10,
+        [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { onScan(*msg); });
+    odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        get_parameter("odom_topic").as_string(), 10,
+        [this](nav_msgs::msg::Odometry::SharedPtr msg) { onOdom(*msg); });
+
+    const auto bounds = origincar_wall::wallBounds(config_);
+    RCLCPP_INFO(get_logger(), "Loaded wall fit; bounds x=[%.3f, %.3f] y=[%.3f, %.3f]",
+                bounds.x_min, bounds.x_max, bounds.y_min, bounds.y_max);
+    RCLCPP_INFO(get_logger(), "Publishing %s; publish_tf=%s", wall_odom_topic_.c_str(), publish_tf_ ? "true" : "false");
+  }
+
+private:
+  void onOdom(const nav_msgs::msg::Odometry &msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    latest_twist_ = msg.twist.twist;
+    has_twist_ = true;
+    const auto current_odom_pose = odomMsgToMapPose(msg, config_);
+    if (!has_wall_pose_)
+    {
+      predicted_pose_ = current_odom_pose;
+      last_wall_pose_ = current_odom_pose;
+      has_wall_pose_ = true;
+    }
+    else if (has_odom_pose_)
+    {
+      predicted_pose_ = origincar_wall::predictPoseFromOdomDelta(last_wall_pose_, last_odom_pose_, current_odom_pose);
+    }
+    else
+    {
+      predicted_pose_ = last_wall_pose_;
+    }
+    last_odom_pose_ = current_odom_pose;
+    has_odom_pose_ = true;
+    has_prediction_ = true;
+  }
+
+  void onScan(const sensor_msgs::msg::LaserScan &msg)
+  {
+    const auto now = get_clock()->now();
+    if (has_last_scan_time_ && (now - last_scan_time_) < min_scan_period_)
+    {
+      return;
+    }
+    last_scan_time_ = now;
+    has_last_scan_time_ = true;
+
+    origincar_wall::Pose2D predicted_pose;
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      if (!has_prediction_)
+      {
+        predicted_pose = {config_.coordinate_frame.odom_zero_map.x, config_.coordinate_frame.odom_zero_map.y, 0.0};
+        predicted_pose_ = predicted_pose;
+        has_prediction_ = true;
+      }
+      else
+      {
+        predicted_pose = predicted_pose_;
+      }
+    }
+
+    const auto scan_points =
+        origincar_wall::transformScanPointsToPoseFrame(scanMsgToPoints(msg, scan_stride_), laser_pose_);
+    const auto result = origincar_wall::localizeFromScan(scan_points, config_, predicted_pose);
+
+    origincar_wall::Pose2D pose_to_publish = result.ok ? result.pose : predicted_pose;
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      last_wall_pose_ = pose_to_publish;
+      predicted_pose_ = pose_to_publish;
+      has_wall_pose_ = true;
+      has_prediction_ = true;
+    }
+    publishWallOdom(pose_to_publish, msg.header.stamp, result.ok);
+    if (publish_tf_)
+    {
+      publishTf(pose_to_publish, msg.header.stamp);
+    }
+    if (!result.ok)
+    {
+      RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 2000, "Wall localization degraded: %s",
+                           result.message.c_str());
+    }
+  }
+
+  void publishWallOdom(const origincar_wall::Pose2D &pose,
+                       const builtin_interfaces::msg::Time &stamp,
+                       bool valid)
+  {
+    nav_msgs::msg::Odometry odom;
+    odom.header.stamp = stamp;
+    odom.header.frame_id = odom_frame_;
+    odom.child_frame_id = base_frame_;
+    odom.pose.pose.position.x = pose.x;
+    odom.pose.pose.position.y = pose.y;
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = yawToQuaternion(pose.theta);
+    odom.pose.covariance[0] = valid ? 0.02 : 0.25;
+    odom.pose.covariance[7] = valid ? 0.02 : 0.25;
+    odom.pose.covariance[35] = valid ? 0.03 : 0.3;
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      if (has_twist_)
+      {
+        odom.twist.twist = latest_twist_;
+      }
+    }
+    wall_odom_pub_->publish(odom);
+  }
+
+  void publishTf(const origincar_wall::Pose2D &pose,
+                 const builtin_interfaces::msg::Time &stamp)
+  {
+    geometry_msgs::msg::TransformStamped transform;
+    transform.header.stamp = stamp;
+    transform.header.frame_id = odom_frame_;
+    transform.child_frame_id = base_frame_;
+    transform.transform.translation.x = pose.x;
+    transform.transform.translation.y = pose.y;
+    transform.transform.translation.z = 0.0;
+    transform.transform.rotation = yawToQuaternion(pose.theta);
+    tf_broadcaster_->sendTransform(transform);
+  }
+
+  std::mutex mutex_;
+  origincar_wall::WallFitConfig config_;
+  std::string wall_odom_topic_;
+  std::string odom_frame_;
+  std::string base_frame_;
+  bool publish_tf_{true};
+  int scan_stride_{2};
+  rclcpp::Duration min_scan_period_{0, 100000000};
+  bool has_last_scan_time_{false};
+  rclcpp::Time last_scan_time_{0, 0, RCL_ROS_TIME};
+  origincar_wall::Pose2D laser_pose_{0.0, 0.0, 0.0};
+  origincar_wall::Pose2D last_odom_pose_{0.0, 0.0, 0.0};
+  origincar_wall::Pose2D last_wall_pose_{0.0, 0.0, 0.0};
+  origincar_wall::Pose2D predicted_pose_{0.0, 0.0, 0.0};
+  bool has_odom_pose_{false};
+  bool has_wall_pose_{false};
+  bool has_prediction_{false};
+  geometry_msgs::msg::Twist latest_twist_;
+  bool has_twist_{false};
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr wall_odom_pub_;
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<WallLocalizationNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/origincar_base/src/wall_preview_server.cpp

@@ -0,0 +1,332 @@
+#include "origincar_base/wall_fit_core.hpp"
+
+#include <nlohmann/json.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <algorithm>
+#include <atomic>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <mutex>
+#include <sstream>
+#include <string>
+#include <thread>
+#include <vector>
+
+namespace
+{
+std::vector<origincar_wall::WallPoint> scanMsgToPoints(const sensor_msgs::msg::LaserScan &msg, int stride)
+{
+  std::vector<origincar_wall::WallPoint> points;
+  stride = std::max(1, stride);
+  points.reserve(msg.ranges.size() / static_cast<std::size_t>(stride) + 1);
+  for (std::size_t index = 0; index < msg.ranges.size(); index += static_cast<std::size_t>(stride))
+  {
+    const auto distance = static_cast<double>(msg.ranges[index]);
+    if (!std::isfinite(distance) || distance < msg.range_min || distance > msg.range_max)
+    {
+      continue;
+    }
+    const auto angle = static_cast<double>(msg.angle_min) + static_cast<double>(index) * msg.angle_increment;
+    points.push_back({distance * std::cos(angle), distance * std::sin(angle)});
+  }
+  return points;
+}
+
+double quaternionToYaw(const geometry_msgs::msg::Quaternion &q)
+{
+  const auto siny_cosp = 2.0 * (q.w * q.z + q.x * q.y);
+  const auto cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
+  return std::atan2(siny_cosp, cosy_cosp);
+}
+
+nlohmann::json poseJson(double x, double y, double theta)
+{
+  return nlohmann::json{{"x", x}, {"y", y}, {"theta", theta}};
+}
+
+std::string readTextFile(const std::string &path)
+{
+  std::ifstream file(path);
+  if (!file.good())
+  {
+    return "";
+  }
+  std::ostringstream stream;
+  stream << file.rdbuf();
+  return stream.str();
+}
+
+std::vector<unsigned char> readBinaryFile(const std::string &path)
+{
+  std::ifstream file(path, std::ios::binary);
+  if (!file.good())
+  {
+    return {};
+  }
+  return std::vector<unsigned char>((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
+}
+
+std::string requestPath(const std::string &request)
+{
+  const auto first_space = request.find(' ');
+  if (first_space == std::string::npos)
+  {
+    return "/";
+  }
+  const auto second_space = request.find(' ', first_space + 1);
+  auto path = request.substr(first_space + 1, second_space - first_space - 1);
+  const auto query = path.find('?');
+  if (query != std::string::npos)
+  {
+    path = path.substr(0, query);
+  }
+  return path;
+}
+}  // namespace
+
+class WallPreviewServer : public rclcpp::Node
+{
+public:
+  WallPreviewServer()
+      : Node("wall_preview_server")
+  {
+    declare_parameter<std::string>("host", "0.0.0.0");
+    declare_parameter<int>("port", 8772);
+    declare_parameter<std::string>("config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
+    declare_parameter<std::string>("preview_dir", "/home/sunrise/yiliao_ws/src/origincar_base/preview");
+    declare_parameter<std::string>("scan_topic", "/scan");
+    declare_parameter<std::string>("combined_odom_topic", "/odom_combined");
+    declare_parameter<int>("scan_stride", 3);
+    declare_parameter<double>("preview_hz", 8.0);
+
+    host_ = get_parameter("host").as_string();
+    port_ = get_parameter("port").as_int();
+    config_path_ = get_parameter("config_path").as_string();
+    preview_dir_ = get_parameter("preview_dir").as_string();
+    scan_stride_ = get_parameter("scan_stride").as_int();
+    preview_period_ms_ = static_cast<int>(1000.0 / std::max(1.0, get_parameter("preview_hz").as_double()));
+    wall_fit_ = origincar_wall::loadWallFitConfig(config_path_);
+
+    combined_odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+        get_parameter("combined_odom_topic").as_string(), 10,
+        [this](nav_msgs::msg::Odometry::SharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          combined_pose_ = poseJson(msg->pose.pose.position.x, msg->pose.pose.position.y,
+                                    quaternionToYaw(msg->pose.pose.orientation));
+          has_combined_pose_ = true;
+        });
+    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+        get_parameter("scan_topic").as_string(), 10,
+        [this](sensor_msgs::msg::LaserScan::SharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          if (!has_combined_pose_)
+          {
+            return;
+          }
+          const origincar_wall::Pose2D pose{
+              combined_pose_.at("x").get<double>(),
+              combined_pose_.at("y").get<double>(),
+              combined_pose_.at("theta").get<double>(),
+          };
+          latest_scan_map_ = origincar_wall::transformScanPointsToMap(scanMsgToPoints(*msg, scan_stride_), pose);
+        });
+
+    server_thread_ = std::thread([this]() { serve(); });
+    RCLCPP_INFO(get_logger(), "Wall preview: http://%s:%d", host_.c_str(), port_);
+  }
+
+  ~WallPreviewServer() override
+  {
+    running_ = false;
+    if (server_fd_ >= 0)
+    {
+      close(server_fd_);
+    }
+    if (server_thread_.joinable())
+    {
+      server_thread_.join();
+    }
+  }
+
+private:
+  nlohmann::json snapshot()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    nlohmann::json scan_points = nlohmann::json::array();
+    for (const auto &point : latest_scan_map_)
+    {
+      scan_points.push_back({{"x", point.x}, {"y", point.y}});
+    }
+    return {
+        {"status", "ok"},
+        {"wall_fit", nlohmann::json::parse(origincar_wall::wallFitConfigToJson(wall_fit_))},
+        {"odom_combined", has_combined_pose_ ? combined_pose_ : nlohmann::json(nullptr)},
+        {"scan_map", {{"points", scan_points}}},
+    };
+  }
+
+  void sendAll(int fd, const std::string &data)
+  {
+    const char *buffer = data.data();
+    std::size_t remaining = data.size();
+    while (remaining > 0)
+    {
+      const auto sent = send(fd, buffer, remaining, MSG_NOSIGNAL);
+      if (sent <= 0)
+      {
+        return;
+      }
+      buffer += sent;
+      remaining -= static_cast<std::size_t>(sent);
+    }
+  }
+
+  void sendBytes(int fd, const std::string &content_type, const std::vector<unsigned char> &bytes)
+  {
+    std::ostringstream header;
+    header << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type
+           << "\r\nContent-Length: " << bytes.size()
+           << "\r\nConnection: close\r\n\r\n";
+    sendAll(fd, header.str());
+    if (!bytes.empty())
+    {
+      send(fd, bytes.data(), bytes.size(), MSG_NOSIGNAL);
+    }
+  }
+
+  void sendText(int fd, const std::string &content_type, const std::string &body)
+  {
+    std::ostringstream response;
+    response << "HTTP/1.1 200 OK\r\nContent-Type: " << content_type
+             << "\r\nContent-Length: " << body.size()
+             << "\r\nConnection: close\r\n\r\n"
+             << body;
+    sendAll(fd, response.str());
+  }
+
+  void sendNotFound(int fd)
+  {
+    sendText(fd, "text/plain; charset=utf-8", "not found\n");
+  }
+
+  void sendEvents(int fd)
+  {
+    sendAll(fd, "HTTP/1.1 200 OK\r\nContent-Type: text/event-stream\r\nCache-Control: no-cache\r\nConnection: keep-alive\r\n\r\n");
+    while (running_ && rclcpp::ok())
+    {
+      const auto payload = std::string("event: snapshot\ndata: ") + snapshot().dump() + "\n\n";
+      if (send(fd, payload.data(), payload.size(), MSG_NOSIGNAL) <= 0)
+      {
+        break;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(preview_period_ms_));
+    }
+  }
+
+  void handleClient(int fd)
+  {
+    std::array<char, 2048> buffer{};
+    const auto received = recv(fd, buffer.data(), buffer.size() - 1, 0);
+    if (received <= 0)
+    {
+      close(fd);
+      return;
+    }
+    const auto path = requestPath(std::string(buffer.data(), static_cast<std::size_t>(received)));
+    if (path == "/" || path == "/wall_localization.html")
+    {
+      const auto html = readTextFile(preview_dir_ + "/wall_localization.html");
+      html.empty() ? sendNotFound(fd) : sendText(fd, "text/html; charset=utf-8", html);
+    }
+    else if (path == "/wall_fit.json")
+    {
+      wall_fit_ = origincar_wall::loadWallFitConfig(config_path_);
+      sendText(fd, "application/json; charset=utf-8", origincar_wall::wallFitConfigToJson(wall_fit_));
+    }
+    else if (path == "/map_reference.png")
+    {
+      const auto bytes = readBinaryFile(preview_dir_ + "/map_reference.png");
+      bytes.empty() ? sendNotFound(fd) : sendBytes(fd, "image/png", bytes);
+    }
+    else if (path == "/events")
+    {
+      sendEvents(fd);
+    }
+    else
+    {
+      sendNotFound(fd);
+    }
+    close(fd);
+  }
+
+  void serve()
+  {
+    server_fd_ = socket(AF_INET, SOCK_STREAM, 0);
+    if (server_fd_ < 0)
+    {
+      RCLCPP_ERROR(get_logger(), "Failed to create preview socket");
+      return;
+    }
+    int opt = 1;
+    setsockopt(server_fd_, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+    sockaddr_in address{};
+    address.sin_family = AF_INET;
+    address.sin_port = htons(static_cast<uint16_t>(port_));
+    address.sin_addr.s_addr = host_ == "0.0.0.0" ? INADDR_ANY : inet_addr(host_.c_str());
+    if (bind(server_fd_, reinterpret_cast<sockaddr *>(&address), sizeof(address)) < 0)
+    {
+      RCLCPP_ERROR(get_logger(), "Failed to bind preview port %d", port_);
+      return;
+    }
+    if (listen(server_fd_, 8) < 0)
+    {
+      RCLCPP_ERROR(get_logger(), "Failed to listen on preview port %d", port_);
+      return;
+    }
+    while (running_ && rclcpp::ok())
+    {
+      const auto client_fd = accept(server_fd_, nullptr, nullptr);
+      if (client_fd < 0)
+      {
+        continue;
+      }
+      std::thread([this, client_fd]() { handleClient(client_fd); }).detach();
+    }
+  }
+
+  std::string host_;
+  int port_{8772};
+  std::string config_path_;
+  std::string preview_dir_;
+  int scan_stride_{3};
+  int preview_period_ms_{125};
+  int server_fd_{-1};
+  std::atomic<bool> running_{true};
+  std::thread server_thread_;
+  std::mutex mutex_;
+  origincar_wall::WallFitConfig wall_fit_;
+  nlohmann::json combined_pose_;
+  bool has_combined_pose_{false};
+  std::vector<origincar_wall::WallPoint> latest_scan_map_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr combined_odom_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+};
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<WallPreviewServer>());
+  rclcpp::shutdown();
+  return 0;
+}

src/origincar_base/test/test_wall_fit_core.py

@@ -0,0 +1,104 @@
+import math
+import tempfile
+import unittest
+from pathlib import Path
+
+from scripts.wall_fit_core import (
+    DEFAULT_REFERENCE_POINTS,
+    DEFAULT_WALL_FIT,
+    fit_wall_config_from_captures,
+    load_wall_fit,
+    localize_from_scan,
+    map_points_to_scan,
+    predict_pose_from_odom_delta,
+    save_wall_fit_atomic,
+    wall_bounds,
+)
+
+
+class WallFitCoreTests(unittest.TestCase):
+    def test_reference_points_use_lower_left_map_frame(self):
+        self.assertEqual(DEFAULT_REFERENCE_POINTS["P"]["map"], {"x": 0.56, "y": 0.18})
+        self.assertEqual(DEFAULT_REFERENCE_POINTS["task_release"]["map"], {"x": 4.5, "y": 1.81})
+        self.assertEqual(DEFAULT_REFERENCE_POINTS["passage"]["map"], {"x": 2.5, "y": 2.6})
+        self.assertEqual(DEFAULT_REFERENCE_POINTS["clinic"]["map"], {"x": 2.8, "y": 3.77})
+
+    def test_predict_pose_from_odom_delta_advances_wall_pose(self):
+        previous_wall_pose = {"x": 1.0, "y": 2.0, "theta": math.radians(170.0)}
+        previous_odom_pose = {"x": 0.56, "y": 0.18, "theta": math.radians(175.0)}
+        current_odom_pose = {"x": 0.86, "y": 0.08, "theta": math.radians(-175.0)}
+
+        predicted = predict_pose_from_odom_delta(previous_wall_pose, previous_odom_pose, current_odom_pose)
+
+        self.assertAlmostEqual(predicted["x"], 1.3, places=6)
+        self.assertAlmostEqual(predicted["y"], 1.9, places=6)
+        self.assertAlmostEqual(predicted["theta"], math.radians(-180.0), places=6)
+        self.assertEqual(predicted["prediction_source"], "odom_delta")
+
+    def test_wall_bounds_can_extend_outside_map_image(self):
+        config = dict(DEFAULT_WALL_FIT)
+        config["walls"] = [
+            {"name": "left", "a": 1.0, "b": 0.0, "c": 0.08, "color": "#2ca02c"},
+            {"name": "right", "a": -1.0, "b": 0.0, "c": 5.12, "color": "#ff7f0e"},
+            {"name": "bottom", "a": 0.0, "b": 1.0, "c": 0.05, "color": "#1f77b4"},
+            {"name": "top", "a": 0.0, "b": -1.0, "c": 5.2, "color": "#d62728"},
+        ]
+
+        bounds = wall_bounds(config)
+
+        self.assertEqual(bounds, {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2})
+
+    def test_localize_from_scan_corrects_xy_and_theta(self):
+        map_points = []
+        for y in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]:
+            map_points.append({"x": 0.0, "y": y})
+            map_points.append({"x": 5.0, "y": y})
+        for x in [0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2]:
+            map_points.append({"x": x, "y": 0.0})
+            map_points.append({"x": x, "y": 5.0})
+        true_pose = {"x": 1.6, "y": 1.5, "theta": math.radians(8.0)}
+        predicted_pose = {"x": 1.55, "y": 1.45, "theta": 0.0}
+        scan = {"points": map_points_to_scan(map_points, true_pose)}
+
+        result = localize_from_scan(scan, DEFAULT_WALL_FIT, predicted_pose)
+
+        self.assertTrue(result["ok"])
+        self.assertAlmostEqual(result["pose"]["x"], true_pose["x"], delta=0.08)
+        self.assertAlmostEqual(result["pose"]["y"], true_pose["y"], delta=0.08)
+        self.assertAlmostEqual(result["pose"]["theta"], true_pose["theta"], delta=math.radians(1.1))
+
+    def test_fit_wall_config_from_captures_uses_outer_points_beyond_map_edges(self):
+        captures = []
+        true_bounds = {"x_min": -0.08, "x_max": 5.12, "y_min": -0.05, "y_max": 5.2}
+        map_points = []
+        for y in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2]:
+            map_points.append({"x": true_bounds["x_min"], "y": y})
+            map_points.append({"x": true_bounds["x_max"], "y": y})
+        for x in [0.7, 1.4, 2.1, 2.8, 3.5, 4.2, 4.9]:
+            map_points.append({"x": x, "y": true_bounds["y_min"]})
+            map_points.append({"x": x, "y": true_bounds["y_max"]})
+        for key, reference in DEFAULT_REFERENCE_POINTS.items():
+            pose = {"x": reference["map"]["x"], "y": reference["map"]["y"], "theta": 0.0}
+            captures.append({"key": key, "map": reference["map"], "odom_pose": pose, "scan": {"points": map_points_to_scan(map_points, pose)}})
+
+        config = fit_wall_config_from_captures(captures, DEFAULT_WALL_FIT)
+        bounds = wall_bounds(config)
+
+        self.assertAlmostEqual(bounds["x_min"], true_bounds["x_min"], delta=0.04)
+        self.assertAlmostEqual(bounds["x_max"], true_bounds["x_max"], delta=0.04)
+        self.assertAlmostEqual(bounds["y_min"], true_bounds["y_min"], delta=0.04)
+        self.assertAlmostEqual(bounds["y_max"], true_bounds["y_max"], delta=0.04)
+
+    def test_save_wall_fit_atomic_round_trips_json(self):
+        with tempfile.TemporaryDirectory() as temp_dir:
+            path = Path(temp_dir) / "wall_fit.json"
+            save_wall_fit_atomic(path, DEFAULT_WALL_FIT)
+
+            loaded = load_wall_fit(path)
+
+        self.assertEqual(loaded["coordinate_frame"]["origin"], "map_lower_left")
+        self.assertEqual(loaded["reference_points"]["P"]["map"], {"x": 0.56, "y": 0.18})
+
+
+if __name__ == "__main__":
+    unittest.main()

src/origincar_base/test/wall_fit_core_test.cpp

@@ -0,0 +1,164 @@
+#include "origincar_base/wall_fit_core.hpp"
+
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace
+{
+constexpr double kPi = 3.14159265358979323846;
+
+void require(bool condition, const std::string &message)
+{
+  if (!condition)
+  {
+    throw std::runtime_error(message);
+  }
+}
+
+void requireNear(double actual, double expected, double tolerance, const std::string &message)
+{
+  if (std::fabs(actual - expected) > tolerance)
+  {
+    throw std::runtime_error(message + " actual=" + std::to_string(actual) +
+                             " expected=" + std::to_string(expected));
+  }
+}
+
+std::vector<origincar_wall::WallPoint> makeWallPoints(const origincar_wall::WallBounds &bounds)
+{
+  std::vector<origincar_wall::WallPoint> points;
+  for (double y : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2})
+  {
+    points.push_back({bounds.x_min, y});
+    points.push_back({bounds.x_max, y});
+  }
+  for (double x : {0.6, 1.2, 1.8, 2.4, 3.0, 3.6, 4.2})
+  {
+    points.push_back({x, bounds.y_min});
+    points.push_back({x, bounds.y_max});
+  }
+  return points;
+}
+
+void testReferencePointsUseLowerLeftMapFrame()
+{
+  const auto config = origincar_wall::defaultWallFitConfig();
+  requireNear(config.reference_points.at("P").map.x, 0.56, 1e-9, "P x");
+  requireNear(config.reference_points.at("P").map.y, 0.18, 1e-9, "P y");
+  requireNear(config.reference_points.at("task_release").map.x, 4.5, 1e-9, "task x");
+  requireNear(config.reference_points.at("task_release").map.y, 1.81, 1e-9, "task y");
+  requireNear(config.reference_points.at("passage").map.x, 2.5, 1e-9, "passage x");
+  requireNear(config.reference_points.at("passage").map.y, 2.6, 1e-9, "passage y");
+  requireNear(config.reference_points.at("clinic").map.x, 2.8, 1e-9, "clinic x");
+  requireNear(config.reference_points.at("clinic").map.y, 3.77, 1e-9, "clinic y");
+}
+
+void testPredictPoseFromOdomDeltaAdvancesWallPose()
+{
+  const origincar_wall::Pose2D previous_wall_pose{1.0, 2.0, 170.0 * kPi / 180.0};
+  const origincar_wall::Pose2D previous_odom_pose{0.56, 0.18, 175.0 * kPi / 180.0};
+  const origincar_wall::Pose2D current_odom_pose{0.86, 0.08, -175.0 * kPi / 180.0};
+
+  const auto predicted =
+      origincar_wall::predictPoseFromOdomDelta(previous_wall_pose, previous_odom_pose, current_odom_pose);
+
+  requireNear(predicted.x, 1.3, 1e-9, "predicted x");
+  requireNear(predicted.y, 1.9, 1e-9, "predicted y");
+  requireNear(predicted.theta, -kPi, 1e-9, "predicted theta");
+}
+
+void testWallBoundsCanExtendOutsideMapImage()
+{
+  auto config = origincar_wall::defaultWallFitConfig();
+  config.walls = {
+      {"left", 1.0, 0.0, 0.08, "#2ca02c"},
+      {"right", -1.0, 0.0, 5.12, "#ff7f0e"},
+      {"bottom", 0.0, 1.0, 0.05, "#1f77b4"},
+      {"top", 0.0, -1.0, 5.2, "#d62728"},
+  };
+
+  const auto bounds = origincar_wall::wallBounds(config);
+
+  requireNear(bounds.x_min, -0.08, 1e-9, "x_min");
+  requireNear(bounds.x_max, 5.12, 1e-9, "x_max");
+  requireNear(bounds.y_min, -0.05, 1e-9, "y_min");
+  requireNear(bounds.y_max, 5.2, 1e-9, "y_max");
+}
+
+void testLocalizeFromScanCorrectsXYAndTheta()
+{
+  const auto config = origincar_wall::defaultWallFitConfig();
+  const auto points = makeWallPoints({0.0, 5.0, 0.0, 5.0});
+  const origincar_wall::Pose2D true_pose{1.6, 1.5, 8.0 * kPi / 180.0};
+  const origincar_wall::Pose2D predicted_pose{1.55, 1.45, 0.0};
+  const auto scan = origincar_wall::mapPointsToScan(points, true_pose);
+
+  const auto result = origincar_wall::localizeFromScan(scan, config, predicted_pose);
+
+  require(result.ok, "localization should be ok");
+  requireNear(result.pose.x, true_pose.x, 0.08, "localized x");
+  requireNear(result.pose.y, true_pose.y, 0.08, "localized y");
+  requireNear(result.pose.theta, true_pose.theta, 1.1 * kPi / 180.0, "localized theta");
+}
+
+void testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges()
+{
+  const auto base_config = origincar_wall::defaultWallFitConfig();
+  const origincar_wall::WallBounds true_bounds{-0.08, 5.12, -0.05, 5.2};
+  const auto points = makeWallPoints(true_bounds);
+  std::vector<origincar_wall::WallFitCapture> captures;
+  for (const auto &entry : base_config.reference_points)
+  {
+    origincar_wall::WallFitCapture capture;
+    capture.key = entry.first;
+    capture.map = entry.second.map;
+    capture.odom_pose = {entry.second.map.x, entry.second.map.y, 0.0};
+    capture.scan_points = origincar_wall::mapPointsToScan(points, capture.odom_pose);
+    captures.push_back(capture);
+  }
+
+  const auto config = origincar_wall::fitWallConfigFromCaptures(captures, base_config);
+  const auto bounds = origincar_wall::wallBounds(config);
+
+  requireNear(bounds.x_min, true_bounds.x_min, 0.04, "fit x_min");
+  requireNear(bounds.x_max, true_bounds.x_max, 0.04, "fit x_max");
+  requireNear(bounds.y_min, true_bounds.y_min, 0.04, "fit y_min");
+  requireNear(bounds.y_max, true_bounds.y_max, 0.04, "fit y_max");
+}
+
+void testSaveWallFitAtomicRoundTripsJson()
+{
+  const std::string path = "/tmp/origincar_wall_fit_core_test.json";
+  const auto config = origincar_wall::defaultWallFitConfig();
+  origincar_wall::saveWallFitConfig(path, config);
+  const auto loaded = origincar_wall::loadWallFitConfig(path);
+  std::remove(path.c_str());
+
+  require(loaded.coordinate_frame.origin == "map_lower_left", "loaded origin");
+  requireNear(loaded.reference_points.at("P").map.x, 0.56, 1e-9, "loaded P x");
+  requireNear(loaded.reference_points.at("P").map.y, 0.18, 1e-9, "loaded P y");
+}
+}  // namespace
+
+int main()
+{
+  try
+  {
+    testReferencePointsUseLowerLeftMapFrame();
+    testPredictPoseFromOdomDeltaAdvancesWallPose();
+    testWallBoundsCanExtendOutsideMapImage();
+    testLocalizeFromScanCorrectsXYAndTheta();
+    testFitWallConfigFromCapturesUsesOuterPointsBeyondMapEdges();
+    testSaveWallFitAtomicRoundTripsJson();
+  }
+  catch (const std::exception &error)
+  {
+    std::cerr << "wall_fit_core_test failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}

src/origincar_base/test/wall_kalman_filter_test.cpp

@@ -0,0 +1,79 @@
+#include "origincar_base/wall_kalman_filter.hpp"
+
+#include <cmath>
+#include <iostream>
+#include <stdexcept>
+#include <string>
+
+namespace
+{
+constexpr double kPi = 3.14159265358979323846;
+
+void requireNear(double actual, double expected, double tolerance, const std::string &message)
+{
+  if (std::fabs(actual - expected) > tolerance)
+  {
+    throw std::runtime_error(message + " actual=" + std::to_string(actual) +
+                             " expected=" + std::to_string(expected));
+  }
+}
+
+void require(bool condition, const std::string &message)
+{
+  if (!condition)
+  {
+    throw std::runtime_error(message);
+  }
+}
+
+void testPredictUsesWheelVelocityAndImuYawRate()
+{
+  origincar_wall::WallKalmanFilter filter({0.56, 0.18, 0.0});
+
+  filter.predict(1.0, 0.0, 0.2, 0.5);
+  const auto pose = filter.pose();
+
+  requireNear(pose.x, 1.06, 1e-9, "predicted x");
+  requireNear(pose.y, 0.18, 1e-9, "predicted y");
+  requireNear(pose.theta, 0.1, 1e-9, "predicted theta");
+}
+
+void testScanUpdateTrustsLowNoiseMoreThanHighNoise()
+{
+  origincar_wall::WallKalmanFilter weak_filter({0.0, 0.0, 0.0});
+  origincar_wall::WallKalmanFilter strong_filter({0.0, 0.0, 0.0});
+  weak_filter.predict(1.0, 0.0, 0.0, 1.0);
+  strong_filter.predict(1.0, 0.0, 0.0, 1.0);
+
+  weak_filter.correct({2.0, 0.0, 0.0}, {1.0, 1.0, 1.0});
+  strong_filter.correct({2.0, 0.0, 0.0}, {0.01, 0.01, 0.01});
+
+  require(strong_filter.pose().x > weak_filter.pose().x, "low scan noise should pull x closer to scan");
+  requireNear(strong_filter.pose().x, 2.0, 0.08, "strong scan update x");
+}
+
+void testYawCorrectionUsesWrappedInnovation()
+{
+  origincar_wall::WallKalmanFilter filter({0.0, 0.0, 179.0 * kPi / 180.0});
+
+  filter.correct({0.0, 0.0, -179.0 * kPi / 180.0}, {0.01, 0.01, 0.01});
+
+  require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
+}
+}  // namespace
+
+int main()
+{
+  try
+  {
+    testPredictUsesWheelVelocityAndImuYawRate();
+    testScanUpdateTrustsLowNoiseMoreThanHighNoise();
+    testYawCorrectionUsesWrappedInnovation();
+  }
+  catch (const std::exception &error)
+  {
+    std::cerr << "wall_kalman_filter_test failed: " << error.what() << std::endl;
+    return 1;
+  }
+  return 0;
+}