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_