┌─────────────────────────────────────────────────┐
│              Localization Stack                 │
├─────────────────────────────────────────────────┤
│                                                 │
│  ┌──────────┐  ┌──────────┐  ┌──────────────┐  │
│  │ Odometry │  │  Vision  │  │   Particle   │  │
│  │   (IMU)  │  │Landmarks │  │   Filter     │  │
│  └────┬─────┘  └────┬─────┘  └──────┬───────┘  │
│       │             │               │          │
│       └─────────────┴───────────────┘          │
│                     │                          │
│                     ▼                          │
│            Robot Position (x, y, θ)            │
│                                                │
└─────────────────────────────────────────────────┘

# Pipeline
Walking Commands → Step Integration → Position Update
IMU Data → Orientation Update → Heading Correction

# Odometry topic
ros2 topic echo /odom

# TF transform
odom → base_link

# nav_msgs/Odometry
pose:
  pose:
    position:
      x: 1.5    # meters
      y: 0.3
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.1    # quaternion
      w: 0.99
twist:
  twist:
    linear:
      x: 0.2    # m/s
    angular:
      z: 0.1    # rad/s

┌────────────────────────────────────────┐
│                                        │
│    ┌────┐              ┌────┐          │
│    │Goal│              │Goal│          │
│    └────┘              └────┘          │
│         ┌──────────────┐               │
│         │              │               │
│         │    ○         │ ← Center      │
│         │  Circle      │               │
│         │              │               │
│         └──────────────┘               │
│                                        │
│    ═══════════════════════════════     │ ← Field Lines
│                                        │
└────────────────────────────────────────┘

def localize_from_lines(detected_lines, map_lines):
    """
    Match detected lines to map lines.
    Estimate robot pose from correspondences.
    """
    # 1. Transform detected lines to robot frame
    # 2. Find matching lines in map
    # 3. Calculate pose from matches
    pass

┌────────────────────────────────────────┐
│           Particle Cloud               │
│                                        │
│     .  . .   .  .                     │
│    . . . . . . . .   ← Particles      │
│     .  .  *  . .        (possible     │
│    . . . . . . .         positions)   │
│     .  . .   .                        │
│                * ← True position      │
│                                        │
└────────────────────────────────────────┘

for each timestep:
    # 1. Motion Update (Prediction)
    for particle in particles:
        particle.move(odometry + noise)

    # 2. Sensor Update (Correction)
    for particle in particles:
        particle.weight = likelihood(sensor_data, particle.pose)

    # 3. Resampling
    particles = resample(particles, weights)

world
  │
  └── map
        │
        └── odom
              │
              └── base_link
                    │
                    ├── camera_link
                    └── imu_link

ros2 run humanoid_planner_2d planner_node

Start Position                    Goal Position
     ○                                 ○
     │                                 │
     ▼                                 ▼
   [Left]  →  [Right]  →  [Left]  →  [Right]
     1          2           3          4

         Y
         ↑
         │         ┌────────────────────┐
         │         │    Opponent Goal   │
         │         └────────────────────┘
         │
    ─────┼─────────────────────────────→ X
         │         (Center of Field)
         │
         │         ┌────────────────────┐
         │         │     Our Goal       │
         │         └────────────────────┘

    X: Positive toward opponent goal
    Y: Positive toward left
    θ: Counter-clockwise from X-axis

# Reset odometry periodically
ros2 service call /reset_odometry std_srvs/srv/Empty

import tf2_ros

# Create TF buffer and listener
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer, node)

# Get transform
transform = tf_buffer.lookup_transform('map', 'base_link', rclpy.time.Time())

# Extract position
x = transform.transform.translation.x
y = transform.transform.translation.y

from nav_msgs.msg import Odometry

odom = Odometry()
odom.header.stamp = self.get_clock().now().to_msg()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.orientation = quaternion

publisher.publish(odom)