[fmrico]

Fixes #157

The MPPI controller rewritten, for differential and holonomic robots, round or not. Needs EasyNavigation/EasyNavigation#153 and EasyNavigation/EasyNavigation#154 (merged).

Motion models (motion_model):

• diff (default): linear.x and angular.z, always facing along the path.
• omni: also linear.y. The control sequence is kept in the map frame, so turning does not change the course. Translation is limited by the ellipse of max_linear_vel (min_linear_vel backwards) and max_lateral_vel: diagonals are not faster than the axes. A zero lateral limit makes it a differential robot.

Heading of a holonomic robot while moving (heading_mode): path (default, facing the path ahead), free, goal, hold (the heading when the goal arrived), fixed (heading_yaw). The NavState key heading_reference (rad, map frame), while present, overrides it. The goal's heading is always reached at the end. A differential robot ignores both, with a warning if configured.

Optimizer:

• 500 rollouts × 40 steps of 0.1 s by default (was 100 × 10). The previous solution is shifted by the elapsed time.
• The noise is correlated in time.
• Deterministic candidates: arcs, turns in place, turn-then-go, eight straight directions (omni), straight to the goal on the last stretch.
• When the weighted average is worse than the best rollout (left and right turns averaging to straight on), the best rollout is used.
• Costs per step: distance to the path, progress along it, goal distance, heading (to a point heading_lookahead ahead), goal heading near the goal (omni), obstacle margin (quadratic), collision, backwards (diff), smoothness. With an obstacle on the path within reach, path and heading weights drop to a hundredth.
• Obstacle and path distances are computed once per cycle in grids, so each rollout step is O(1).
• Both motion models use the same default weights (path 40, heading 20).

Obstacles and shape:

• Perception points all around the robot within obstacle_range (fov is deprecated: it warns, no effect).
• With robot_geometry.footprint, the polygon is checked turned with each rollout, exactly in the band where the circles cannot decide. Its own radii are used, not the configured ones (which may be planning margins).

Goal: turns in place to the goal's heading, braking in time, once at the position; stops within both tolerances (the strictest of the plugin's and the system's). Reports blocked when every rollout collides.

Limits:

• The controller's limits are controller_node.robot_limits, lateral ones included.
• They are re-read every cycle, so a safely limited speed applies at once.
• The old plugin limit parameters still work, with a deprecation warning.

Configuration impact: existing configs keep working as diff. The playground configs are updated in easynav_playgrounds (following PR).

• Built as Release when no build type is given: unoptimized, the rollouts do not fit in a control cycle.

Tests

• mppi_optimizer_tests (19): limits, ellipse, zero-lateral axis, warm start, arrival, alignment, blocked, the distance grids.
• mppi_scenarios_tests: a kinematic simulator with the controller's velocity smoother, in 6 configurations (diff; omni in each heading mode) × 16 paths (straight, L, U, backwards, sideways, diagonal, S, loose goal, turn in place, obstacle, narrow corridor, L corridor, wall, long path, loop), checking arrival, no collision, limits and heading while moving. Plus rectangular robots: narrow corridor, a door that requires turning, turning next to a wall.
• mppi_controller_modes_tests (8): parameters, unknown modes, commands per model and mode, heading_reference, idle, footprint.
• mppi_controller_obstacle_points_tests: points all around.

Also checked in Gazebo: Kobuki (8 goals), omni playground in path and goal modes (6 + 6), TIAGo Pro in path and goal modes (5 + 5), all reached without collisions.

The scenario matrix takes about 6 minutes.