Performance Optimization

# config/nav2_params.yaml

controller_server:
  ros__parameters:
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"

      # Linear velocity (forward/back, strafe)
      min_vel_x: -0.5       # Backward speed
      max_vel_x: 0.7        # ↑ Increase for speed (from 0.5)
      min_vel_y: -0.5       # Strafe left
      max_vel_y: 0.5        # Strafe right
      max_speed_xy: 0.75    # ↑ Max combined XY speed

      # Angular velocity (rotation)
      max_vel_theta: 1.2    # ↑ Increase for faster turns (from 1.0)
      min_speed_theta: 0.0

      # Acceleration limits
      acc_lim_x: 3.0        # ↑ Faster acceleration (from 2.5)
      acc_lim_y: 3.0
      acc_lim_theta: 3.5    # ↑ Faster rotation accel
      decel_lim_x: -3.0
      decel_lim_y: -3.0
      decel_lim_theta: -3.5

planner_server:
  ros__parameters:
    expected_planner_frequency: 0.5  # ↓ Reduce from 1.0 Hz
                                     # Less frequent replanning

    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.75          # ↑ Increase tolerance (from 0.5)
                              # Allows reaching "close enough"
      use_astar: true          # ↑ Enable A* (faster than Dijkstra)
      allow_unknown: true

# If using Smac Planner
SmacPlanner:
  plugin: "nav2_smac_planner/SmacPlanner2D"
  max_iterations: 500000     # ↓ Reduce (from 1000000)
  max_planning_time: 3.0     # ↓ Reduce timeout (from 5.0)

  smooth_path: true          # Keep enabled for smooth paths
  smoother:
    max_iterations: 500      # ↓ Reduce smoothing iterations
    w_smooth: 0.3
    w_data: 0.2

controller_server:
  ros__parameters:
    FollowPath:
      # Velocity sampling (trajectory rollout)
      vx_samples: 15         # ↓ Reduce from 20 (faster compute)
      vy_samples: 15         # ↓ Reduce from 20
      vtheta_samples: 30     # ↓ Reduce from 40

      sim_time: 1.5          # ↓ Reduce lookahead (from 1.7)

      # Controller frequency
      controller_frequency: 25.0  # ↓ Reduce from 20 Hz if CPU limited
                                 # ↑ Increase to 30 Hz for smoother control

      # Emphasize path following vs. obstacle avoidance
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle",
                "GoalAlign", "PathAlign", "PathDist", "GoalDist"]

      # Adjust weights for behavior
      PathAlign.scale: 40.0      # ↑ Stick closer to path (from 32)
      GoalDist.scale: 30.0       # ↑ Prioritize reaching goal (from 24)
      BaseObstacle.scale: 0.01   # ↓ Less conservative around obstacles (from 0.02)

local_costmap:
  local_costmap:
    ros__parameters:
      # Update rates
      update_frequency: 4.0      # ↓ Reduce from 5.0 Hz
      publish_frequency: 2.0     # Keep at 2.0 Hz

      # Costmap size (smaller = faster)
      width: 3                   # Keep small for local
      height: 3
      resolution: 0.05           # Or increase to 0.10 for faster

      # Plugin configuration
      plugins: ["voxel_layer", "inflation_layer"]

      voxel_layer:
        enabled: true
        mark_threshold: 0
        observation_sources: scan

        scan:
          obstacle_max_range: 2.5   # ↓ Reduce from 3.0 (less data)
          raytrace_max_range: 3.0   # Keep for clearing

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 0.5      # ↓ Reduce from 1.0 Hz
      publish_frequency: 0.5

      # Static + obstacle + inflation
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

      obstacle_layer:
        enabled: true
        observation_sources: scan
        scan:
          obstacle_max_range: 11.0  # ↓ Slightly reduce from 11.5

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.50     # ↓ Reduce for less conservative (from 0.55)
        cost_scaling_factor: 3.0   # Adjust for obstacle cost decay

# Monitor CPU per process
top -p $(pgrep -d',' -f ros2)

# Watch for processes using >50% CPU:
# - /controller_server (trajectory rollout)
# - /planner_server (path planning)
# - /slam_toolbox (if mapping)
# - /camera_node (if using camera)

free -h

# Watch specific processes
watch -n 1 'ps aux --sort=-%mem | head -n 10'

# Test procedure:
# 1. Mark starting position
# 2. Command robot to move exactly 5.0m forward
# 3. Measure actual distance traveled

# Calculate correction:
new_radius = old_radius * (measured_dist / commanded_dist)

mecanum_drive_controller:
  ros__parameters:
    wheel_radius: 0.052  # Adjusted value

ekf_filter_node:
  ros__parameters:
    # Process noise covariance (how much to trust motion model)
    process_noise_covariance: [
      0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
      0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
      0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
      # ... (increase values if motion is unpredictable)
    ]

    # Odometry sensor variance (measurement noise)
    odom0_config: [true, true, false, ...]
    odom0_differential: false
    odom0_relative: false

    odom0_queue_size: 2

    # IMU sensor variance
    imu0_config: [false, false, false,
                  false, false, true,  # Use yaw
                  false, false, false,
                  false, false, true,  # Use yaw_dot
                  false, false, false]
    imu0_differential: false
    imu0_relative: true
    imu0_queue_size: 5
    imu0_remove_gravitational_acceleration: true

# Lower velocity limits
max_vel_x: 0.4        # From 0.5-0.7
max_vel_theta: 0.8    # From 1.0-1.2

# Lower acceleration (smoother = less current spikes)
acc_lim_x: 2.0        # From 3.0
acc_lim_theta: 2.5    # From 3.5

# Reduce computational load (see CPU optimization above)
# Lower CPU = lower Pi power draw

# Disable camera if not needed (saves ~5W)
# Lower LiDAR scan rate (if firmware supports)

// ESP32: Monitor battery voltage
float battery_voltage = analogRead(BATTERY_PIN) * (12.6 / 4095.0) * 3.0;

if (battery_voltage < 10.5) {  // 3.5V per cell (low)
  Serial.println("WARNING: Low battery!");
  // Reduce max velocity or return to charging station
}

amcl:
  ros__parameters:
    # Particle filter
    min_particles: 500       # ↓ Reduce for speed (from 500)
    max_particles: 2000      # ↓ Reduce for speed (from 5000)

    # Recovery (if localization lost)
    recovery_alpha_slow: 0.001  # Increase for faster recovery
    recovery_alpha_fast: 0.1

    # Update thresholds (when to resample)
    update_min_d: 0.25       # ↑ Less frequent updates (from 0.2)
    update_min_a: 0.2        # ↑ Less frequent (from 0.5)

    # Laser model
    laser_max_range: 12.0
    laser_min_range: 0.2
    laser_max_beams: 60      # ↓ Use subset of scan (from 100)

slam_toolbox:
  ros__parameters:
    # Solver plugin
    solver_plugin: solver_plugins::CeresSolver

    # Performance
    minimum_travel_distance: 0.25  # ↑ Map less frequently (from 0.2)
    minimum_travel_heading: 0.3    # ↑ Map less frequently (from 0.2)
    map_update_interval: 2.0       # ↑ Update map less often (from 1.0)

    # Loop closure
    do_loop_closing: true
    loop_search_maximum_distance: 3.0  # ↓ Search closer loops (from 5.0)

#!/usr/bin/env python3
import time
import subprocess

def measure_cpu():
    result = subprocess.run(['top', '-bn1'], capture_output=True, text=True)
    # Parse CPU usage from result.stdout
    return cpu_percent

def measure_battery_voltage():
    # Read from ESP32 or voltage sensor
    return voltage

def benchmark_navigation(goals):
    results = {
        'successes': 0,
        'failures': 0,
        'avg_time': 0,
        'cpu_avg': 0,
        'battery_start': measure_battery_voltage(),
        'battery_end': 0
    }

    start_time = time.time()

    for goal in goals:
        cpu_before = measure_cpu()

        # Send navigation goal
        success = navigate_to_pose(goal)

        cpu_after = measure_cpu()
        results['cpu_avg'] += (cpu_after + cpu_before) / 2

        if success:
            results['successes'] += 1
        else:
            results['failures'] += 1

    results['avg_time'] = (time.time() - start_time) / len(goals)
    results['cpu_avg'] /= len(goals)
    results['battery_end'] = measure_battery_voltage()

    print_results(results)
    return results