Implement Feature Rich STANLEY Pathfollower

[ClayJay3]

Path Follower Namespace

Overview

Objective

The pathfollower namespace introduces a lateral Stanley controller class, providing a precise trajectory following mechanism. This controller enhances vehicle navigation, ensuring accurate lateral control during path following.

Stanley Controller Class

• The Stanley controller is the cornerstone of the pathfollower namespace.
• It implements a lateral control mechanism based on the Stanley control algorithm.
• Designed for precise trajectory following, especially in scenarios with curvature.
• When following a path the controller should not directly control the rover or send anything over rovecomm. ABSTRACT IT! Instead return a heading and drive power and let the state machine send the drive commands.

Implementation Details

Class Structure

• The pathfollower namespace is currently structured around the Stanley controller class, but should allow for more algorithms to be added in the future.
• Designed for extensibility, allowing seamless integration of new lateral control algorithms in the future.

Controller Logic

• The Stanley controller autonomously adjusts the vehicle's steering angle for optimal trajectory following.
• Integrates lateral control mechanisms tailored for dynamic path following scenarios.

Waypoint Integration

• The stanley controller should strictly work with UTMCoordinate structs internally, but should be able to take in both UTM and GPS. Look at the geoops namespaces for conversion.
• Method: SetPath(std::vector<geoops::UTMCoordinates or geoops::GPSCoordinate> vVector) for including waypoints in the path.

Path Following Algorithm

• The lateral Stanley controller dynamically adjusts the vehicle's path, taking into account waypoint locations.
• The controller ensures precise navigation, particularly in scenarios with varying curvature.