Initial Challenges: When SLAM Goes Wrong

The Symptoms

My SLAM system showed classic drift symptoms that pointed to deeper issues:

Laser Scans Moving with Robot: In RViz, the pink laser scan dots moved with the robot instead of remaining fixed in the map frame.
Frame Separation: I noticed three distinct coordinate origins — the robot, odometry frame, and map frame — all offset from each other.
Transform Extrapolation Errors: RViz logs were filled with timing-related errors.
Map Distortion: The map looked okay when stationary but warped during motion.

The Confusion

What confused me most: laser scans drifted during straight-line motion but partially realigned during rotation. Initially, I mistook this for working loop closure. But after deeper analysis, I realized loop closure is not functioning properly yet. The system is trying — but it isn’t successfully closing loops or correcting long-term drift.

🔍 Diagnostic Process: Systematic Debugging

Step 1: Transform Tree Analysis

I inspected the transform tree:

Key Finding: The transform chain (map → odom → base_footprint → base_link → laser) was structurally correct, but timestamps were often misaligned, causing extrapolation errors.

# Check TF tree structure
ros2 run tf2_tools view_frames
# Monitor specific frame relationships
ros2 run tf2_ros tf2_echo map odom
ros2 run tf2_ros tf2_echo base_footprint base_link
ros2 run tf2_ros tf2_echo base_link laser

Step 2: Topic Rate and Timestamp Validation

I measured the topic rates and confirmed mismatches between TF publishing and sensor data timestamps.

# Check component frequencies
ros2 topic hz /scan # Result: 7.5Hz (good)
ros2 topic hz /tf # Result: 70Hz (excellent) ros2 topic hz /map # Result: 0.2Hz (every 5 seconds)
ros2 topic hz /odom # Result: 10Hz (adequate)

Step 3: Odometry Accuracy Evaluation

To understand how much odometry might be contributing to drift, I tested it in isolation:

Commanded: 1.0m forward
Actual: 1.073m forward
Error: ~7.3% — surprisingly decent for a Create 2

# Linear accuracy test
echo "Starting position:"
ros2 topic echo /odom --once | grep -A3 "position:"

# Move exactly 1 meter forward
timeout 20s ros2 topic pub /cmd_vel_nav geometry_msgs/msg/Twist \
  "{linear: {x: 0.05, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" --rate 10

echo "Final position:"
ros2 topic echo /odom --once | grep -A3 "position:"

Step 4: SLAM Correction Attempt Monitoring

I tried to monitor how well SLAM was correcting drift over short tests:

Before Movement: [0.000, 0.000, -0.017]
After Movement: [-0.020, 0.023, -0.017]
The small correction suggests some adjustment, but not the kind of large-scale loop closure I expected.

# Monitor map-odom transform during movement
ros2 run tf2_ros tf2_echo map odom

🛠️ Partial Fixes and Improvements So Far

✅ Transform Timing Stabilized

I improved the startup order and transform publication frequency, which eliminated most extrapolation errors.

# Correct startup order
# Terminal 1: Start robot first, wait for full initialization
ros2 launch perceptor launch_robot.launch.py enable_camera:=false

# Terminal 2: Start SLAM after robot is ready  
ros2 launch slam_toolbox online_async_launch.py \
  params_file:=/home/pi/Roomba/slam_dev_ws/src/perceptor/config/mapper_params_online_async.yaml \
  use_sim_time:=false

# Terminal 3: Start RViz last
ros2 run rviz2 rviz2 -d src/perceptor/config/map.rviz

✅ Removed Feedback Loop in Twist_Mux

I had incorrectly set both input and output of twist_mux to /cmd_vel, creating an infinite feedback loop. Separating them fixed the phantom motion issue.

# config/twist_mux.yaml - BEFORE (broken)
navigation:
  topic: cmd_vel      # ❌ Same as output - creates feedback loop!
  timeout: 0.5
  priority: 10

# AFTER (fixed)  
navigation:
  topic: cmd_vel_nav  # ✅ Separate input topic
  timeout: 0.5
  priority: 10

✅ Simplified Configuration Files

Merged redundant joystick configuration files for better maintainability.

# config/pro_controller.yaml - Consolidated Nintendo Pro Controller config
joy_teleop:
  ros__parameters:
    axis_linear:
      x: 1    # Left stick vertical
    scale_linear:
      x: 0.4  # Fast mode: 0.4 m/s, Slow mode: 0.2 m/s  
    axis_angular:
      yaw: 0  # Left stick horizontal
    scale_angular:
      yaw: 2.4  # Fast mode: 2.4 rad/s, Slow mode: 1.2 rad/s
    enable_button: 3  # Y button deadman switch
    enable_turbo_button: -1

🛠️ Still Unresolved: Loop Closure

Despite these improvements, the SLAM drift continues to accumulate, and loop closure is not triggering as expected. Maps don’t realign reliably after revisiting known locations. Laser scans still exhibit visible drift over longer distances. Tried the following but did not help.

# config/mapper_params_online_async.yaml - Optimized for poor odometry
slam_toolbox:
  ros__parameters:
    # Faster transform publishing for better RViz sync
    transform_publish_period: 0.02  # Was: 0.05
    transform_timeout: 0.1          # Was: 0.2
    
    # More frequent map updates  
    map_update_interval: 2.0        # Was: 5.0
    
    # Reduce odometry reliance, increase scan matching
    minimum_travel_distance: 0.05   # Was: 0.5
    minimum_travel_heading: 0.05    # Was: 0.5
    distance_variance_penalty: 0.1  # Was: 0.5
    angle_variance_penalty: 0.1     # Was: 1.0
    
    # Correct frame configuration
    base_frame: base_link           # Was: base_footprint
    max_laser_range: 12.0          # Match RPLidar A1M8 specs

⚠️ Key Takeaways (So Far)

1. Not All Drift Is a Bug — But This One Is

SLAM does allow small drift during straight-line motion, but in my case, the system isn’t correcting it later. Loop closure is either failing to trigger or not correcting the map correctly.

2. TF Synchronization Issues Were a Real Problem

Fixing transform timing helped reduce errors in RViz and made debugging easier — but didn’t fix the core SLAM issue.

3. Good Odometry Helps, But Isn’t Enough

Even with decent odometry (~7% error), the SLAM system needs successful loop closures to stay accurate over time.

🔄 Next Steps

Tune SLAM Parameters Further: I suspect loop_closure_detection_frequency, transform_timeout, and other parameters may need to be better tuned for my platform.
Enable SLAM Toolbox Debug Output: To confirm whether loop closure candidates are being rejected or missed entirely.
Visualize Pose Graph: I want to use tools like RViz or SLAM Toolbox’s pose graph visualizer to see whether loop closure edges are being added at all.
Add IMU or Fiducial Markers: If loop closure continues to fail, I may try sensor fusion (IMU) or use AprilTags for artificial loop closures.

💭 In Summary

I’ve addressed some foundational system-level bugs — transform timing, configuration loops, and odometry validation — but true loop closure remains unresolved. Until I see consistent corrections during revisited areas, I won’t consider SLAM working as intended.

This is still very much a work in progress, and I’ll continue refining parameters and adding tools to get to robust, drift-free mapping.