feat: add a progress check so the controller can abort execution when it's stuck

[JustusBraun]

Does what the title says.
Also fixes a bug in the cost function calculation.

This feature adds two 3 parameters:
A threshold for distance traveled
A threshold for % trajectory cost reduction
And a timeframe to monitor

If both thresholds are violated in the monitored timeframe the robot is considered stuck and an appropriate outcome is reported in the ExePath action.

[Copilot]

Pull request overview

This PR adds a progress check feature to detect when the robot is stuck and also fixes a bug in the XTE cost function calculation.

Key Changes:

• Implements progress monitoring using translation distance and trajectory cost reduction thresholds over a configurable timeframe
• Fixes bug in XTECostFunction where plan_polyline_ was incorrectly used instead of ref_ and corrects distance calculation
• Replaces first_ flag and reached_goal_ boolean with a proper state machine (IDLE, MOVING, REACHED_GOAL, FAILED_GOAL)

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 9 comments.

Show a summary per file

File	Description
mesh_mppi/src/scoring/XTECostFunction.cpp	Fixes cost calculation bug by using correct reference polyline and distance method; adds documentation for edge case
mesh_mppi/src/controller/MeshMPPIBase.cpp	Adds progress check parameters, implements isMakingProgress() method, integrates state machine, and adds centralized logger
mesh_mppi/include/mesh_mppi/optimizer/MPPIOptimizer.inl	Updates Result initialization to include cost field using designated initializers
mesh_mppi/include/mesh_mppi/optimizer/MPPIOptimizer.hpp	Adds cost field to Result struct for progress tracking
mesh_mppi/include/mesh_mppi/controller/MeshMPPIBase.hpp	Adds StateMachine enum, progress check members, logger, and removes first_ and reached_goal_ flags
mesh_mppi/include/mesh_mppi/controller/MeshMPPI.inl	Integrates progress check, changes future handling logic, adds cost extraction, and updates to use centralized logger
mesh_mppi/include/mesh_mppi/controller/MeshMPPI.hpp	Implements resetFuture() method required by base class
README.md	Documents the three new progress check parameters with descriptions and default values

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[JustusBraun]

Fixes #4