kernel_function.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_GLOBAL_PLANNER_KERNEL_FUNCTION_H
36 #define DLUX_GLOBAL_PLANNER_KERNEL_FUNCTION_H
37 
39 
40 namespace dlux_global_planner
41 {
42 /*
43  Flag Mask Values such that NORTH + WEST = NORTHWEST
44 */
46 {
47  STATIONARY = 0, // 0000
48  NORTH = 1, // 0001
49  SOUTH = 2, // 0010
50  WEST = 4, // 0100
51  NORTHWEST = 5, // 0101
52  SOUTHWEST = 6, // 0110
53  EAST = 8, // 1000
54  NORTHEAST = 9, // 1001
55  SOUTHEAST = 10 // 1010
56 };
57 
59 {
60  return static_cast<CardinalDirection>(static_cast<int>(a) + static_cast<int>(b));
61 }
62 
64 {
65  return (static_cast<int>(a) & static_cast<int>(b)) > 0;
66 }
67 
83 static float calculateKernel(const PotentialGrid& potential_grid, unsigned char cost, unsigned int x, unsigned int y,
84  CardinalDirection* upstream = nullptr)
85 {
86  // See README.md for more about this calculation
87  // get neighboring potential values
88  float south_p = y > 0 ? potential_grid(x, y - 1) : HIGH_POTENTIAL;
89  float north_p = y < potential_grid.getHeight() - 1 ? potential_grid(x, y + 1) : HIGH_POTENTIAL;
90  float west_p = x > 0 ? potential_grid(x - 1, y) : HIGH_POTENTIAL;
91  float east_p = x < potential_grid.getWidth() - 1 ? potential_grid(x + 1, y) : HIGH_POTENTIAL;
92 
93  // Find the lowest neighbor on each axis
94  // pa = min(P(A), P(B)) (aka north_p and south_p)
95  // pc = min(P(C), P(D)) (aka west_p and east_p)
96  float pa, pc;
97  CardinalDirection xdir, ydir;
98  if (north_p < south_p)
99  {
100  pa = north_p;
102  }
103  else
104  {
105  pa = south_p;
107  }
108 
109  if (west_p < east_p)
110  {
111  pc = west_p;
113  }
114  else
115  {
116  pc = east_p;
118  }
119 
120  // cost was originally notated hf, h in the README
121  float dc = pc - pa; // relative cost between pa, pc
122  CardinalDirection mindir;
123 
124  // In our calculation, we assume P(A) <= P(C), so we flip as needed
125  if (pa == HIGH_POTENTIAL || dc < 0) // pc is lowest
126  {
127  dc = -dc;
128  pa = pc;
129  mindir = xdir;
130  }
131  else
132  {
133  mindir = ydir;
134  }
135 
136  // If pa is lower and STILL infinite, then we can't calculate a good potential here
137  if (std::isinf(pa))
138  {
139  if (upstream)
140  *upstream = CardinalDirection::STATIONARY;
141  return pa;
142  }
143 
144  // calculate new potential
145  if (dc >= cost)
146  {
147  // if the difference is too large, use the "straightforward" calculation
148  if (upstream)
149  *upstream = mindir;
150  return pa + cost;
151  }
152  else
153  {
154  // Otherwise, interpolate from both neighbors
155  float dx = dc / cost;
156  float v = -0.2301 * dx * dx + 0.5307 * dx + 0.7040;
157  if (upstream)
158  *upstream = xdir + ydir;
159  return pa + cost * v;
160  }
161 }
162 
163 
164 } // namespace dlux_global_planner
165 
166 #endif // DLUX_GLOBAL_PLANNER_KERNEL_FUNCTION_H
bool operator&(CardinalDirection a, CardinalDirection b)
const float HIGH_POTENTIAL
Definition: potential.h:46
CardinalDirection operator+(CardinalDirection a, CardinalDirection b)
static float calculateKernel(const PotentialGrid &potential_grid, unsigned char cost, unsigned int x, unsigned int y, CardinalDirection *upstream=nullptr)
potential calculation that uses multiple values of the neighboring cells


dlux_global_planner
Author(s):
autogenerated on Sun Jan 10 2021 04:08:52