Sim localization

The MAP panel on /sim/[robotId] switches between building a grid and estimating pose on a frozen grid. Both paths run in the browser. This page does not open a socket.

Modes

LocalizationSource.mode is mapping or localization.

  • Mapping (simulated SLAM) — InBrowserMapSource keeps integrating planar scans. The occupancy grid, costmaps, and travelled path keep updating.
  • Localize on saved map — the current grid is copied and frozen, or the user loads map.pgm and map.yaml. The copy is parsed with readMapServer (the same map_server reader the save button uses). The live grid is not written. Switching back to mapping resumes it from the cells it already had.

Loading a file selects localization and keeps that file until Freeze built map snapshots the live grid again. A bad file leaves the current mode and the live grid unchanged.

Particle filter

ParticleFilter is the in-browser estimator. The default cloud is 300 particles. The count, motion noise, sensor mixture, and initial-pose sigmas are constructor options.

  • Motion. The increment is the simulator base pose from the previous update to this one, expressed in the previous body frame (x forward, y left). Each particle applies that increment in its own body frame. Translation and yaw noise are Gaussian. The scales are alphaTrans, alphaTransRot, transFloor, alphaYaw, alphaYawTrans, and yawFloor. A step with no motion adds no noise.
  • Measurement. A ring of ranges is cast with the mapping ray walker (rayCellsInGrid) from the simulator pose against the sim obstacles. Bearings match the mapper: beam i is at yaw + (i + 0.5) * 2π / N - π. Each hit is scored on a likelihood field, the Euclidean distance from the beam endpoint to the nearest occupied cell (occupancy >= 65). The mixture is zHit * exp(-d² / 2σ²) + zRand. A max-range beam uses zMax and does not consult the field. Weights are normalised with a log-sum-exp.
  • Resampling. Low-variance resampling (one uniform draw in [0, 1/N), then a stride of 1/N) runs when the effective sample size falls below half the cloud. Output weights are uniform and sum to 1.
  • Estimate. Weighted mean of x and y. Yaw is the weighted circular mean. Covariance is the weighted 3×3 of [x, y, yaw], row-major.
  • Initial pose. setInitialPose draws a Gaussian around the given x, y, and yaw and sets the state back to initialising. The numeric fields on the MAP panel call this. A click-drag pose tool is not part of this panel.
  • State. initialising after a new prior, while the cloud is still wide, or when the frozen grid has no occupied cells. tracking when the position spread is under 0.45 m and the hit beams agree with the map. lost when the hit beams disagree with a map that does contain obstacles, or the spread exceeds 1.6 m.

The viewport shows the estimate error against the simulator pose, in metres, a state pill, and the particle cloud as a map layer. The panel copy is "in-browser estimator (simulated)".

LocalizationSource

interface LocalizationSource {
  readonly kind: "in-browser" | "rosbridge";
  readonly mode: "mapping" | "localization";
  readonly state: "initialising" | "tracking" | "lost";
  readonly estimate: { x: number; y: number; yaw: number };
  readonly covariance: readonly [
    number, number, number,
    number, number, number,
    number, number, number,
  ]; // row-major x, y, yaw
  setInitialPose(pose: { x: number; y: number; yaw: number }): void;
}

InBrowserLocalizationSource is the in-browser kind. RosbridgeLocalizationSource is the rosbridge kind. The MAP panel constructs only the in-browser source.

Rosbridge stub

RosbridgeLocalizationSource accepts already-decoded JSON. Constructing it does not open a socket, subscribe, or apply TF.

Mapping mode is the slam_toolbox side. The grid is still nav_msgs/OccupancyGrid on /map, documented in docs/sim-mapping.md. This stub does not build that grid. Switching the stub to mapping clears the particle list and returns the state to initialising.

Localization mode is AMCL.

Topic Message Role
/amcl_pose geometry_msgs/PoseWithCovarianceStamped ingestAmclPose copies the pose and the planar part of the covariance
/initialpose geometry_msgs/PoseWithCovarianceStamped setInitialPose stores the message a publisher would send
/particle_cloud geometry_msgs/PoseArray ingestParticleCloud copies the poses

Nav2 AMCL's default cloud topic is /particlecloud. This shape uses /particle_cloud. A future bridge can alias the two; the stub does not subscribe to either.

PoseWithCovarianceStamped as rosbridge JSON:

interface RosbridgePoseWithCovarianceStamped {
  header: { stamp: { sec: number; nanosec: number }; frame_id: string };
  pose: {
    pose: {
      position: { x: number; y: number; z: number };
      orientation: { x: number; y: number; z: number; w: number };
    };
    covariance: number[]; // length 36, row-major 6×6: x, y, z, roll, pitch, yaw
  };
}

The planar 3×3 reads covariance indices 0 (x x), 1 (x y), 5 (x yaw), 6 (y x), 7 (y y), 11 (y yaw), 30 (yaw x), 31 (yaw y), and 35 (yaw yaw). Yaw comes from the quaternion. z, roll, and pitch are ignored.

PoseArray as rosbridge JSON:

interface RosbridgePoseArray {
  header: { stamp: { sec: number; nanosec: number }; frame_id: string };
  poses: Array<{
    position: { x: number; y: number; z: number };
    orientation: { x: number; y: number; z: number; w: number };
  }>;
}

ingestAmclPose sets the state to tracking when xx + yy is at most 2, and to lost when it is larger. The frame id is stored on the message and is not transformed.

All docs