a_star.hpp

// Copyright (c) 2020, Samsung Research America
// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
 
#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
#define NAV2_SMAC_PLANNER__A_STAR_HPP_
 
#include <functional>
#include <memory>
#include <queue>
#include <tuple>
#include <utility>
#include <vector>
 
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_core/planner_exceptions.hpp"
 
#include "nav2_smac_planner/thirdparty/robin_hood.h"
#include "nav2_smac_planner/analytic_expansion.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/node_basic.hpp"
#include "nav2_smac_planner/goal_manager.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
 
namespace nav2_smac_planner
{
 
template<typename NodeT>
class AStarAlgorithm
{
public:
  typedef NodeT * NodePtr;
  typedef robin_hood::unordered_node_map<uint64_t, NodeT> Graph;
  typedef std::vector<NodePtr> NodeVector;
  typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
  typedef typename NodeT::Coordinates Coordinates;
  typedef typename NodeT::CoordinateVector CoordinateVector;
  typedef typename NodeVector::iterator NeighborIterator;
  typedef std::function<bool (const uint64_t &, NodeT * &)> NodeGetter;
  typedef GoalManager<NodeT> GoalManagerT;
  using NodeContext = typename NodeT::NodeContext;
 
 
  struct NodeComparator
  {
    bool operator()(const NodeElement & a, const NodeElement & b) const
    {
      return a.first > b.first;
    }
  };
 
  typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
 
  explicit AStarAlgorithm(const MotionModel & motion_model, const SearchInfo & search_info);
 
  ~AStarAlgorithm();
 
  void initialize(
    const bool & allow_unknown,
    int & max_iterations,
    const int & max_on_approach_iterations,
    const int & terminal_checking_interval,
    const double & max_planning_time,
    const float & lookup_table_size,
    const unsigned int & dim_3_size);
 
  bool createPath(
    CoordinateVector & path, int & num_iterations, const float & tolerance,
    std::function<bool()> cancel_checker,
    std::vector<std::tuple<float, float, float>> * expansions_log = nullptr);
 
  void setSearchInfo(const SearchInfo & search_info) {_search_info = search_info;}
 
  void setCollisionChecker(GridCollisionChecker * collision_checker);
 
  void setGoal(
    const float & mx,
    const float & my,
    const unsigned int & dim_3,
    const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT,
    const int & coarse_search_resolution = 1);
 
  void setStart(
    const float & mx,
    const float & my,
    const unsigned int & dim_3);
 
  int & getMaxIterations();
 
  NodePtr & getStart();
 
  int & getOnApproachMaxIterations();
 
  float & getToleranceHeuristic();
 
  unsigned int & getSizeX();
 
  unsigned int & getSizeY();
 
  unsigned int & getSizeDim3();
 
  unsigned int getCoarseSearchResolution();
 
  GoalManagerT getGoalManager();
 
  NodeContext * getContext();
 
protected:
  inline NodePtr getNextNode();
 
  inline void addNode(const float & cost, NodePtr & node);
 
  inline NodePtr addToGraph(const uint64_t & index);
 
  inline float getHeuristicCost(const NodePtr & node);
 
  inline bool areInputsValid();
 
  inline bool getClosestPathWithinTolerance(CoordinateVector & path);
 
  inline void clearQueue();
 
  inline void clearGraph();
 
  inline uint64_t getIndex(
    const unsigned int & x, const unsigned int & y, const unsigned int & dim3);
 
  inline bool onVisitationCheckNode(const NodePtr & node);
 
  inline void populateExpansionsLog(
    const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
 
  bool _traverse_unknown;
  bool _is_initialized;
  int _max_iterations;
  int _max_on_approach_iterations;
  int _terminal_checking_interval;
  double _max_planning_time;
  float _tolerance;
  unsigned int _x_size;
  unsigned int _y_size;
  unsigned int _dim3_size;
  unsigned int _coarse_search_resolution;
  SearchInfo _search_info;
 
  NodePtr _start;
  GoalManagerT _goal_manager;
  Graph _graph;
  NodeQueue _queue;
 
  MotionModel _motion_model;
  NodeHeuristicPair _best_heuristic_node;
 
  GridCollisionChecker * _collision_checker;
  nav2_costmap_2d::Costmap2D * _costmap;
  std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
  std::shared_ptr<NodeContext> _shared_ctx;
};
 
}  // namespace nav2_smac_planner
 
#include "nav2_smac_planner/a_star_impl.hpp"  // NOLINT
 
#endif  // NAV2_SMAC_PLANNER__A_STAR_HPP_