Mppi Trajectory Visualization

name: mppi-trajectory-visualization description: 'Visualize MPPI trajectory rollouts. Use when debugging MPPI controller behavior, viewing sampled trajectories, or analyzing critic scoring.'

MPPI Trajectory Visualization

Enabling Visualization

Set visualize: true in the MPPI controller configuration:

controller_server:
  ros__parameters:
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      visualize: true
      # ... other params

Published Topics

When visualization is enabled, MPPI publishes:

RViz2 Setup

1. Open RViz2 and click Add → By topic.
2. Expand /mppi_controller/trajectories and select MarkerArray.
3. Expand /mppi_controller/optimal_trajectory and select MarkerArray.
4. Optionally add /mppi_controller/transformed_global_plan as a Path display.

Recommended RViz2 display settings:

MarkerArray (trajectories):
  Topic: /mppi_controller/trajectories
  Marker Alpha: 0.3  (reduce opacity to avoid visual clutter)
  
MarkerArray (optimal):
  Topic: /mppi_controller/optimal_trajectory
  Marker Alpha: 1.0

Color Coding

Trajectories are colored by cost using a gradient:

• Green: Low cost — good trajectories that satisfy critics.
• Yellow: Moderate cost — suboptimal but viable.
• Red: High cost — near-collision, constraint violation, or heavily penalized.

The optimal trajectory (the weighted average of good trajectories) is displayed separately and is typically a distinct color (blue or bright green).

Interpreting Healthy Visualization

Healthy Patterns

• Fan-shaped spread: Trajectories radiate forward and to the sides from the robot, forming a fan. The green trajectories cluster around the path, red trajectories are off-path or near obstacles. This indicates good sampling diversity and proper critic balance.
• Smooth convergence: Frame-to-frame, the optimal trajectory shifts smoothly. No sudden jumps.
• Green tunnel through obstacles: In narrow passages, green trajectories form a narrow band through the corridor center. Red trajectories hit walls on either side.

Problematic Patterns

All Trajectories Red

• Cause: Critics are too punishing OR costmap is too restrictive (inflation too wide) OR the robot is near-surrounded by obstacles.
• Fix: Reduce CostCritic.cost_weight, reduce costmap inflation radius, increase batch_size to find viable gaps.
• Danger: If all trajectories are red, MPPI may command zero velocity (stuck) or produce erratic motion.

Trajectories Cluster Very Tightly

• Cause: vx_std and/or wz_std are too small. The Gaussian perturbations don't explore enough of the trajectory space.
• Fix: Increase vx_std (e.g., 0.2 → 0.3) and wz_std (e.g., 0.4 → 0.6). Also ensure batch_size is sufficient.

Trajectories Oscillate Frame-to-Frame

• Cause: temperature too high (averaging too many mediocre trajectories) OR conflicting critic weights causing the optimal to flip between two competing solutions.
• Fix: Lower temperature (e.g., 0.3 → 0.15). Check for critic weight conflicts, especially PathAlignCritic vs CostCritic near obstacles.

Trajectories Extend Too Far or Too Short

• Cause: time_steps × model_dt is mismatched to the environment.
• Fix: For indoor use, a 2–3 second horizon is typical. Adjust time_steps or model_dt.

Trajectories Curve Away from the Path

• Cause: PathAlignCritic.cost_weight too low relative to CostCritic — the costmap is repelling the robot from the path.
• Fix: Increase PathAlignCritic.cost_weight or reduce CostCritic.cost_weight or reduce inflation.

Optimal Trajectory Passes Through Obstacles

• Cause: CostCritic.cost_weight too low OR trajectory_point_step too high (skipping collision checks) OR costmap not up-to-date.
• Fix: Increase CostCritic.cost_weight, set trajectory_point_step: 1, verify costmap update rate.

CPU Impact

Visualization adds overhead:

• Constructing MarkerArray messages for batch_size trajectories × time_steps points: 5–15ms per cycle.
• Network serialization and RViz2 rendering: depends on subscriber.
• For 2000 trajectories × 56 time steps = 112,000 markers per message.

Always disable for production (visualize: false). The overhead can push the controller computation over the frequency budget.

Diagnostic Workflow

1. Set visualize: true and open RViz2 with the trajectory MarkerArray.
2. Command a navigation goal through a challenging area (doorway, corner, narrow hall).
3. Observe:
   • Is there a clear green "best corridor" through obstacles?
   • Does the optimal trajectory track the planned path?
   • Are there sudden jumps in the optimal trajectory?
4. If problems are visible, adjust critic weights and re-test.
5. Compare with the global plan (Path display) to verify alignment.
6. Once satisfied, disable visualization.

Recording for Off-line Analysis

ros2 bag record /mppi_controller/trajectories /mppi_controller/optimal_trajectory \
  /local_costmap/costmap /plan -o mppi_debug_bag

Replay with:

ros2 bag play mppi_debug_bag

Then open RViz2 and add the topics for frame-by-frame analysis.

Combining with Costmap Visualization

For full diagnostic visibility, also display:

• /local_costmap/costmap (OccupancyGrid) — see what the controller sees.
• /global_costmap/costmap — see what the planner sees.
• /plan (Path) — the planned global path.
• /local_plan (Path) — the controller's local trajectory command.

Overlay all four with the MPPI trajectory markers for a complete picture of the controller's decision-making.