General Testing

[Hiptostee]

• Benchmark critical slope (currently 30 deg)
• Best way to aggregate new costmap values with local stored cache (eg avg, always keep newest, etc.)
• How to deal with cliffs and vertical drops
• Create benchmark goals for autonomy testing
• Critical cost threshold in A*
• Tune hyperparameters in costmap generation