AStar.h

Go to the documentation of this file.

 
#ifndef ASTAR_H
#define ASTAR_H
 
#include "../../util/planners/Nodes.hpp"
 
// Put implicit includes in here.
#include <sl/Camera.hpp>
 
 
 
namespace pathplanners
{
 
    class AStar
    {
        public:
            // Declare public member variables.
 
            // Declare public primary methods.
            AStar();
            ~AStar();
            std::vector<geoops::Waypoint> PlanAvoidancePath(const geoops::Waypoint& stStartCoordinate, const geoops::Waypoint& stGoalCoordinate);
            std::vector<geoops::Waypoint> PlanAvoidancePath(const geoops::UTMCoordinate& stStartCoordinate, const geoops::UTMCoordinate& stGoalCoordinate);
            std::vector<geoops::Waypoint> PlanAvoidancePath(const geoops::GPSCoordinate& stStartCoordinate, const geoops::GPSCoordinate& stGoalCoordinate);
            void CancelPathGeneration();
 
            // Setters.
            void UpsertObstacleData(const geoops::Waypoint& stObstacle);
            void UpsertObstacleData(const geoops::UTMCoordinate& stObstacle);
            void UpsertObstacleData(const geoops::GPSCoordinate& stObstacle);
            void UpsertObstacleData(const std::vector<geoops::Waypoint>& vObstacles);
            void UpsertObstacleData(const std::vector<geoops::UTMCoordinate>& vObstacles);
            void UpsertObstacleData(const std::vector<geoops::GPSCoordinate>& vObstacles);
            void ClearObstacleData();
 
            // Getters.
            std::vector<geoops::Waypoint> GetPath() const;
            std::vector<geoops::Waypoint> GetObstacleData() const;
 
        private:
            // Declare private member variables.
            // Start and Goal Nodes
            nodes::AStarNode m_stStartNode;
            nodes::AStarNode m_stGoalNode;
            // Nodes used as the final path for routing
            std::vector<geoops::Waypoint> m_vPathCoordinates;
            // Obstacles for AStar to use during routing
            std::vector<geoops::Waypoint> m_vObstacles;
            // Time point for measuring total planning time.
            std::chrono::steady_clock::time_point m_tmStartTime;
            std::atomic_bool m_bPathGenerationCancelled;
 
            // Declare private methods.
            geoops::Waypoint FindNearestGoalPoint(const geoops::UTMCoordinate& stGoalCoordinate);
            geoops::Waypoint FindNearestStartPoint(const geoops::UTMCoordinate& stStartCoordinate);
            void RoundUTMCoordinate(geoops::UTMCoordinate& stCoordinateToRound);
            void ConstructPath(const nodes::AStarNode& stFinalNode);
            std::string UTMCoordinateToString(const geoops::UTMCoordinate& stToTranslate);
            bool ValidCoordinate(const double dEasting, const double dNorthing);
    };
}    // namespace pathplanners
 
#endif