astar.cpp
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 #include <dlux_plugins/astar.h>
37 #include <nav_core2/exceptions.h>
39 #include <math.h>
41 
43 
44 namespace dlux_plugins
45 {
46 void AStar::initialize(ros::NodeHandle& private_nh, nav_core2::Costmap::Ptr costmap,
48 {
49  cost_interpreter_ = cost_interpreter;
50  private_nh.param("manhattan_heuristic", manhattan_heuristic_, false);
51  private_nh.param("use_kernel", use_kernel_, true);
52  private_nh.param("minimum_requeue_change", minimum_requeue_change_, 1.0);
53 }
54 
55 unsigned int AStar::updatePotentials(dlux_global_planner::PotentialGrid& potential_grid,
56  const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal)
57 {
58  const nav_grid::NavGridInfo& info = potential_grid.getInfo();
59  queue_ = AStarQueue();
60  potential_grid.reset();
61 
62  nav_grid::Index goal_i;
63  worldToGridBounded(info, goal.x, goal.y, goal_i.x, goal_i.y);
64  queue_.push(QueueEntry(goal_i, 0.0));
65  potential_grid.setValue(goal_i, 0.0);
66 
67  // bounds check done in dlux_global_planner
68  nav_grid::Index start_i;
69  worldToGridBounded(info, start.x, start.y, start_i.x, start_i.y);
70 
71  if (potential_grid.getWidth() == 0 || potential_grid.getHeight() == 0)
72  {
73  return 0;
74  }
75 
76  unsigned int width_bound = potential_grid.getWidth() - 1, height_bound = potential_grid.getHeight() - 1;
77  unsigned int c = 0;
78 
79  while (queue_.size() > 0)
80  {
81  QueueEntry top = queue_.top();
82  queue_.pop();
83  c++;
84 
85  nav_grid::Index i = top.i;
86  if (i == start_i) return c;
87 
88  double prev_potential = potential_grid(i);
89 
90  if (i.x < width_bound)
91  add(potential_grid, prev_potential, nav_grid::Index(i.x + 1, i.y), start_i);
92  if (i.x > 0)
93  add(potential_grid, prev_potential, nav_grid::Index(i.x - 1, i.y), start_i);
94  if (i.y < height_bound)
95  add(potential_grid, prev_potential, nav_grid::Index(i.x, i.y + 1), start_i);
96  if (i.y > 0)
97  add(potential_grid, prev_potential, nav_grid::Index(i.x, i.y - 1), start_i);
98  }
99 
101 }
102 
103 void AStar::add(dlux_global_planner::PotentialGrid& potential_grid, double prev_potential,
104  const nav_grid::Index& index, const nav_grid::Index& start_index)
105 {
106  float cost = cost_interpreter_->getCost(index.x, index.y);
107  if (cost_interpreter_->isLethal(cost))
108  return;
109 
110  float new_potential;
111  if (use_kernel_)
112  {
113  new_potential = dlux_global_planner::calculateKernel(potential_grid, cost, index.x, index.y);
114  }
115  else
116  {
117  new_potential = prev_potential + cost;
118  }
119 
120  if (new_potential >= potential_grid(index) || potential_grid(index) - new_potential < minimum_requeue_change_)
121  return;
122 
123  potential_grid.setValue(index, new_potential);
124  queue_.push(QueueEntry(index, new_potential + getHeuristicValue(index, start_index)));
125 }
126 
127 inline unsigned int uintDiff(const unsigned int a, const unsigned int b)
128 {
129  return (a > b) ? a - b : b - a;
130 }
131 
132 float AStar::getHeuristicValue(const nav_grid::Index& index, const nav_grid::Index& start_index) const
133 {
134  unsigned int dx = uintDiff(start_index.x, index.x);
135  unsigned int dy = uintDiff(start_index.y, index.y);
136  float distance;
137  if (manhattan_heuristic_)
138  distance = static_cast<float>(dx + dy);
139  else
140  distance = hypot(dx, dy);
141  return distance * cost_interpreter_->getNeutralCost();
142 }
143 
144 } // namespace dlux_plugins
Helper Class for sorting indexes by their heuristic.
Definition: astar.h:80
Potential calculator that explores using a distance heuristic (A*) but not the kernel function...
Definition: astar.h:49
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
void reset() override
std::priority_queue< QueueEntry, std::vector< QueueEntry >, QueueEntryComparator > AStarQueue
Definition: astar.h:100
bool param(const std::string &param_name, T &param_val, const T &default_val) const
unsigned int getHeight() const
void setValue(const unsigned int x, const unsigned int y, const T &value) override
unsigned int uintDiff(const unsigned int a, const unsigned int b)
Definition: astar.cpp:127
std::shared_ptr< CostInterpreter > Ptr
unsigned int getWidth() const
NavGridInfo getInfo() const
static float calculateKernel(const PotentialGrid &potential_grid, unsigned char cost, unsigned int x, unsigned int y, CardinalDirection *upstream=nullptr)
std::shared_ptr< Costmap > Ptr
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)


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