astar.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #ifndef DLUX_PLUGINS_ASTAR_H
36 #define DLUX_PLUGINS_ASTAR_H
37 
39 #include <queue>
40 #include <vector>
41 
42 namespace dlux_plugins
43 {
44 
50 {
51 public:
52  // Main PotentialCalculator interface
53  void initialize(ros::NodeHandle& private_nh, nav_core2::Costmap::Ptr costmap,
54  dlux_global_planner::CostInterpreter::Ptr cost_interpreter) override;
55  unsigned int updatePotentials(dlux_global_planner::PotentialGrid& potential_grid,
56  const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal) override;
57 protected:
66  void add(dlux_global_planner::PotentialGrid& potential_grid, double prev_potential,
67  const nav_grid::Index& index, const nav_grid::Index& start_index);
68 
75  float getHeuristicValue(const nav_grid::Index& index, const nav_grid::Index& start_index) const;
76 
80  struct QueueEntry
81  {
82  public:
83  QueueEntry(nav_grid::Index index, float heuristic) : i(index), cost(heuristic) {}
85  float cost;
86  };
87 
92  {
93  bool operator()(const QueueEntry& a, const QueueEntry& b) const
94  {
95  return a.cost > b.cost;
96  }
97  };
98 
99  // Indexes sorted by heuristic
100  using AStarQueue = std::priority_queue<QueueEntry, std::vector<QueueEntry>, QueueEntryComparator>;
105 };
106 } // namespace dlux_plugins
107 
108 #endif // DLUX_PLUGINS_ASTAR_H
AStarQueue queue_
Definition: astar.h:101
bool operator()(const QueueEntry &a, const QueueEntry &b) const
Definition: astar.h:93
unsigned int updatePotentials(dlux_global_planner::PotentialGrid &potential_grid, const geometry_msgs::Pose2D &start, const geometry_msgs::Pose2D &goal) override
Definition: astar.cpp:55
Helper Class for sorting indexes by their heuristic.
Definition: astar.h:80
double minimum_requeue_change_
Definition: astar.h:104
Potential calculator that explores using a distance heuristic (A*) but not the kernel function...
Definition: astar.h:49
float getHeuristicValue(const nav_grid::Index &index, const nav_grid::Index &start_index) const
Calculate the heuristic value for a particular cell.
Definition: astar.cpp:132
Comparator for sorting the QueueEntrys.
Definition: astar.h:91
std::priority_queue< QueueEntry, std::vector< QueueEntry >, QueueEntryComparator > AStarQueue
Definition: astar.h:100
std::shared_ptr< CostInterpreter > Ptr
bool manhattan_heuristic_
Definition: astar.h:102
std::shared_ptr< Costmap > Ptr
QueueEntry(nav_grid::Index index, float heuristic)
Definition: astar.h:83
void initialize(ros::NodeHandle &private_nh, nav_core2::Costmap::Ptr costmap, dlux_global_planner::CostInterpreter::Ptr cost_interpreter) override
Definition: astar.cpp:46
void add(dlux_global_planner::PotentialGrid &potential_grid, double prev_potential, const nav_grid::Index &index, const nav_grid::Index &start_index)
Calculate the potential for index if not calculated already.
Definition: astar.cpp:103


dlux_plugins
Author(s):
autogenerated on Sun Jan 10 2021 04:08:54