Context

Wally (wall-mounted system) uses RPLidar C1 to track a guide rail for precise positioning.

Constraints:

• Continuous lidar operation creates audible motor noise—unacceptable for consumer products
• Lidar motor lifespan shortens with continuous operation
• Lidar startup takes ~3 seconds (init + stabilization)

Goal: Achieve instant motion response without sacrificing positioning accuracy from lidar.

---

Core Problem

Waiting for lidar to become valid before starting motion adds 3-second latency—unacceptable for user experience.

Two issues:

1. Startup latency: Lidar needs time to spin up, stabilize, and pass debounce checks
2. Reliability tradeoff: Lidar-only approach fails when data is temporarily invalid (occlusion, etc.)

Key insight

Odom provides immediate position estimates (no startup delay), but drifts over time. Lidar provides absolute position but needs startup time.

Solution: Use odom for immediate response, correct drift with lidar when available.

---

Approach

1) Odom-first startup with background lidar init

When motion command received:

Condition	Action
Y/angle already calibrated	Start immediately with odom
First motion (uncalibrated)	Wait for lidar to get initial calibration

After first calibration, all subsequent motions start with odom—no lidar wait.

---

2) Seamless source switching

Input source changes based on guide_valid flag:

Lidar Status	Position Source	Angle Source
Valid	Lidar direct	Lidar direct
Invalid	Odom + stored correction	Odom + stored correction

Correction storage: When lidar is valid, update map_to_odom offsets (x, y, yaw). When invalid, apply stored corrections to odom.

---

3) Lidar lifecycle management (LidarFsm)

RPLidar’s provided node doesn’t support lifecycle control—implemented custom state machine:

WaitDevice → Init → Running ⇄ Idle

• Idle state: Motor stopped when not needed—eliminates audible noise
• Debounce: 2x count for initial detection, 1x for subsequent
• Lidar-agnostic: Designed to easily swap lidar hardware via handler callbacks

---

Tradeoffs & Limitations

• Initial motion with odom: Less accurate until lidar correction arrives (~3 seconds)
• Correction drift: If lidar stays invalid, stored correction becomes stale

---

Validation

Continuous field testing on deployed Wally units verified:

• Odom-first startup works reliably across power cycles
• Source switching is seamless (no visible position jumps)
• Idle state eliminates motor noise complaints

---

Results

Metric	Before	After
Motion start latency	~3 sec (wait for lidar)	0 sec (odom-first)
Lidar correction arrival	-	~3 sec average
Motor noise when idle	Continuous	Silent

• Seamless transition: Source switches (odom ↔ lidar) without position discontinuity
• Lidar lifespan: Extended via Idle state management

---

Key Takeaway

Odom-first with lidar correction achieves both instant responsiveness and absolute accuracy:

• Start with odom immediately, correct with lidar when available
• Store corrections for fallback during lidar dropout
• LidarFsm manages lidar lifecycle (noise reduction + longevity)