Program Listing for File a_star.hpp

↰ Return to documentation for file (include/nav2_smac_planner/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 <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include <tuple>
#include "Eigen/Core"

#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/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;

  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 setCollisionChecker(GridCollisionChecker * collision_checker);

  void setGoal(
    const float & mx,
    const float & my,
    const unsigned int & dim_3);

  void setStart(
    const float & mx,
    const float & my,
    const unsigned int & dim_3);

  int & getMaxIterations();

  NodePtr & getStart();

  NodePtr & getGoal();

  int & getOnApproachMaxIterations();

  float & getToleranceHeuristic();

  unsigned int & getSizeX();

  unsigned int & getSizeY();

  unsigned int & getSizeDim3();

protected:
  inline NodePtr getNextNode();

  inline void addNode(const float & cost, NodePtr & node);

  inline NodePtr addToGraph(const uint64_t & index);

  inline bool isGoal(NodePtr & node);

  inline float getHeuristicCost(const NodePtr & node);

  inline bool areInputsValid();

  inline void clearQueue();

  inline void clearGraph();

  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;
  SearchInfo _search_info;

  Coordinates _goal_coordinates;
  NodePtr _start;
  NodePtr _goal;

  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;
};

}  // namespace nav2_smac_planner

#endif  // NAV2_SMAC_PLANNER__A_STAR_HPP_