dijkstra.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/dijkstra.h>
37 #include <nav_core2/exceptions.h>
40 #include <queue>
41 
43 
44 namespace dlux_plugins
45 {
46 unsigned int Dijkstra::updatePotentials(dlux_global_planner::PotentialGrid& potential_grid,
47  const geometry_msgs::Pose2D& start, const geometry_msgs::Pose2D& goal)
48 {
49  const nav_grid::NavGridInfo& info = potential_grid.getInfo();
50  queue_ = std::queue<nav_grid::Index>();
51  potential_grid.reset();
52 
53  nav_grid::Index goal_i;
54  worldToGridBounded(info, goal.x, goal.y, goal_i.x, goal_i.y);
55  queue_.push(goal_i);
56  potential_grid.setValue(goal_i, 0.0);
57 
58  nav_grid::Index start_i;
59  worldToGridBounded(info, start.x, start.y, start_i.x, start_i.y);
60  unsigned int c = 0;
61 
62  while (!queue_.empty())
63  {
64  nav_grid::Index i = queue_.front();
65  queue_.pop();
66  c++;
67 
68  if (i == start_i) return c;
69 
70  if (i.x > 0)
71  add(potential_grid, nav_grid::Index(i.x - 1, i.y));
72  if (i.y > 0)
73  add(potential_grid, nav_grid::Index(i.x, i.y - 1));
74 
75  if (i.x < info.width - 1)
76  add(potential_grid, nav_grid::Index(i.x + 1, i.y));
77  if (i.y < info.height - 1)
78  add(potential_grid, nav_grid::Index(i.x, i.y + 1));
79  }
80 
82 }
83 
84 void Dijkstra::add(dlux_global_planner::PotentialGrid& potential_grid, nav_grid::Index next_index)
85 {
86  if (potential_grid(next_index.x, next_index.y) < dlux_global_planner::HIGH_POTENTIAL)
87  return;
88 
89  float cost = cost_interpreter_->getCost(next_index.x, next_index.y);
90  if (cost_interpreter_->isLethal(cost))
91  return;
92  potential_grid.setValue(next_index,
93  dlux_global_planner::calculateKernel(potential_grid, cost, next_index.x, next_index.y));
94  queue_.push(next_index);
95 }
96 
97 } // namespace dlux_plugins
bool add(const actionlib::TwoIntsGoal &req, actionlib::TwoIntsResult &res)
void reset() override
const float HIGH_POTENTIAL
void setValue(const unsigned int x, const unsigned int y, const T &value) override
NavGridInfo getInfo() const
static float calculateKernel(const PotentialGrid &potential_grid, unsigned char cost, unsigned int x, unsigned int y, CardinalDirection *upstream=nullptr)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
Potential calculator that explores the potential breadth first while using the kernel function...
Definition: dijkstra.h:47
bool worldToGridBounded(const NavGridInfo &info, double wx, double wy, unsigned int &mx, unsigned int &my)


dlux_plugins
Author(s):
autogenerated on Wed Jun 26 2019 20:06:32