cost_interpretation_tables.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 NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H
36 #define NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H
37 
38 #include <vector>
39 
40 namespace nav_grid_pub_sub
41 {
45 static std::vector<unsigned char> RAW(0);
46 
52 static std::vector<unsigned char> NEGATE =
53 {
54  // [0, 255] --> 255 - i
55  255, 254, 253, 252, 251, 250, 249, 248, 247, 246, // [0, 9]
56  245, 244, 243, 242, 241, 240, 239, 238, 237, 236,
57  235, 234, 233, 232, 231, 230, 229, 228, 227, 226,
58  225, 224, 223, 222, 221, 220, 219, 218, 217, 216,
59  215, 214, 213, 212, 211, 210, 209, 208, 207, 206,
60  205, 204, 203, 202, 201, 200, 199, 198, 197, 196, // [50, 59]
61  195, 194, 193, 192, 191, 190, 189, 188, 187, 186,
62  185, 184, 183, 182, 181, 180, 179, 178, 177, 176,
63  175, 174, 173, 172, 171, 170, 169, 168, 167, 166,
64  165, 164, 163, 162, 161, 160, 159, 158, 157, 156,
65  155, 154, 153, 152, 151, 150, 149, 148, 147, 146, // [100, 109]
66  145, 144, 143, 142, 141, 140, 139, 138, 137, 136,
67  135, 134, 133, 132, 131, 130, 129, 128, 127, 126,
68  125, 124, 123, 122, 121, 120, 119, 118, 117, 116,
69  115, 114, 113, 112, 111, 110, 109, 108, 107, 106,
70  105, 104, 103, 102, 101, 100, 99, 98, 97, 96, // [150, 159]
71  95, 94, 93, 92, 91, 90, 89, 88, 87, 86,
72  85, 84, 83, 82, 81, 80, 79, 78, 77, 76,
73  75, 74, 73, 72, 71, 70, 69, 68, 67, 66,
74  65, 64, 63, 62, 61, 60, 59, 58, 57, 56,
75  55, 54, 53, 52, 51, 50, 49, 48, 47, 46, // [200, 209]
76  45, 44, 43, 42, 41, 40, 39, 38, 37, 36,
77  35, 34, 33, 32, 31, 30, 29, 28, 27, 26,
78  25, 24, 23, 22, 21, 20, 19, 18, 17, 16,
79  15, 14, 13, 12, 11, 10, 9, 8, 7, 6,
80  5, 4, 3, 2, 1, 0 // [250, 255]
81 };
82 
88 std::vector<unsigned char> getOccupancyInput(bool trinary = false, bool use_unknown_value = false);
89 
97 static std::vector<unsigned char> OCC_GRID_PUBLISHING =
98 {
99  // 0: NO obstacle
100  0,
101  // [1, 252]: scale to [1, 98] i.e. static_cast<char>(1 + (97 * (i - 1)) / 251)
102  1, 1, 1, 2, 2, 2, 3, 3, 4, 4, // [1, 10]
103  4, 5, 5, 6, 6, 6, 7, 7, 7, 8, // [11, 20]
104  8, 9, 9, 9, 10, 10, 11, 11, 11, 12,
105  12, 12, 13, 13, 14, 14, 14, 15, 15, 16,
106  16, 16, 17, 17, 18, 18, 18, 19, 19, 19, // [41, 50]
107  20, 20, 21, 21, 21, 22, 22, 23, 23, 23,
108  24, 24, 24, 25, 25, 26, 26, 26, 27, 27,
109  28, 28, 28, 29, 29, 29, 30, 30, 31, 31,
110  31, 32, 32, 33, 33, 33, 34, 34, 35, 35,
111  35, 36, 36, 36, 37, 37, 38, 38, 38, 39, // [91, 100]
112  39, 40, 40, 40, 41, 41, 41, 42, 42, 43,
113  43, 43, 44, 44, 45, 45, 45, 46, 46, 46,
114  47, 47, 48, 48, 48, 49, 49, 50, 50, 50,
115  51, 51, 52, 52, 52, 53, 53, 53, 54, 54,
116  55, 55, 55, 56, 56, 57, 57, 57, 58, 58, // [141, 150]
117  58, 59, 59, 60, 60, 60, 61, 61, 62, 62,
118  62, 63, 63, 63, 64, 64, 65, 65, 65, 66,
119  66, 67, 67, 67, 68, 68, 69, 69, 69, 70,
120  70, 70, 71, 71, 72, 72, 72, 73, 73, 74,
121  74, 74, 75, 75, 75, 76, 76, 77, 77, 77, // [191, 200]
122  78, 78, 79, 79, 79, 80, 80, 80, 81, 81,
123  82, 82, 82, 83, 83, 84, 84, 84, 85, 85,
124  86, 86, 86, 87, 87, 87, 88, 88, 89, 89,
125  89, 90, 90, 91, 91, 91, 92, 92, 92, 93,
126  93, 94, 94, 94, 95, 95, 96, 96, 96, 97, // [241, 250]
127  97, 98, // [251, 252]
128  // 253: inscribed obstacle
129  99,
130  // 254: lethal obstacle
131  100,
132  // 255: unknown
133  255
134 };
135 
141 std::vector<unsigned char> pixelColoringInterpretation(const double free_threshold,
142  const double occupied_threshold);
143 
149 std::vector<unsigned char> grayScaleInterpretation(const double free_threshold, const double occupied_threshold);
150 
156 static std::vector<unsigned char> TRINARY_SAVE =
157 {
158  // 0: NO obstacle --> white
159  255,
160  // [1, 253] --> gray
161  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [1, 10]
162  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [11, 20]
163  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
164  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
165  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [41, 50]
166  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
167  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
168  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
169  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
170  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [91, 100]
171  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [101, 110]
172  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
173  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
174  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
175  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [141, 150]
176  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
177  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
178  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
179  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
180  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [191, 200]
181  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
182  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
183  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
184  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
185  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [241, 250]
186  205, 205, 205, // [251, 253]
187  // 254: --> black
188  0,
189  // 255 --> gray
190  205
191 };
192 
198 static std::vector<unsigned char> SCALE_SAVE =
199 {
200  // 0: NO obstacle --> white
201  254,
202  // [1, 99] --> scale from white to black
203  252, 250, 247, 245, 242, 240, 237, 234, 232, 229, // [1, 10]
204  227, 224, 221, 219, 216, 214, 211, 209, 206, 203,
205  201, 198, 196, 193, 190, 188, 185, 183, 180, 178,
206  175, 172, 170, 167, 165, 162, 159, 157, 154, 152,
207  149, 147, 144, 141, 139, 136, 134, 131, 129, 126, // [41, 50]
208  123, 121, 118, 116, 113, 110, 108, 105, 103, 100,
209  98, 95, 92, 90, 87, 85, 82, 79, 77, 74,
210  72, 69, 67, 64, 61, 59, 56, 54, 51, 48,
211  46, 43, 41, 38, 36, 33, 30, 28, 25, 23,
212  20, 17, 15, 12, 10, 7, 5, 2, 1, // [91, 99]
213  // 100 --> black
214  0,
215  // [101, 255] --> grey
216  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [101, 110]
217  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
218  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
219  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
220  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [141, 150]
221  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
222  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
223  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
224  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
225  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [191, 200]
226  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
227  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
228  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
229  205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
230  205, 205, 205, 205, 205, 205, 205, 205, 205, 205, // [241, 250]
231  205, 205, 205, 205, 205 // [251, 255]
232 };
233 
234 } // namespace nav_grid_pub_sub
235 
236 #endif // NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H
nav_grid_pub_sub::OCC_GRID_PUBLISHING
static std::vector< unsigned char > OCC_GRID_PUBLISHING
Scale [0, 255] down to [0, 100] (except for a few special values)
Definition: cost_interpretation_tables.h:97
nav_grid_pub_sub::TRINARY_SAVE
static std::vector< unsigned char > TRINARY_SAVE
0 becomes 254 (white), 100 becomes 0 (black) and everything else is 205 (gray)
Definition: cost_interpretation_tables.h:156
nav_grid_pub_sub::pixelColoringInterpretation
std::vector< unsigned char > pixelColoringInterpretation(const double free_threshold, const double occupied_threshold)
Above occupied_threshold is occupied, below free_threshold is free, and the middle gray is unknown...
Definition: cost_interpretation_tables.cpp:41
nav_grid_pub_sub::SCALE_SAVE
static std::vector< unsigned char > SCALE_SAVE
0 becomes 254 (white), 100 becomes 0 (black) and everything in between is gray
Definition: cost_interpretation_tables.h:198
nav_grid_pub_sub::grayScaleInterpretation
std::vector< unsigned char > grayScaleInterpretation(const double free_threshold, const double occupied_threshold)
Above occupied_threshold is occupied, below free_threshold is free, and the middle is in between...
Definition: cost_interpretation_tables.cpp:63
nav_grid_pub_sub::NEGATE
static std::vector< unsigned char > NEGATE
Every value is 255 - i value.
Definition: cost_interpretation_tables.h:52
nav_grid_pub_sub::getOccupancyInput
std::vector< unsigned char > getOccupancyInput(bool trinary=false, bool use_unknown_value=false)
Above 100 is occupied, -1 is sometimes unknown, and otherwise it is either scaled or freespace...
Definition: cost_interpretation_tables.cpp:86
nav_grid_pub_sub::RAW
static std::vector< unsigned char > RAW(0)
Every value is untouched.
nav_grid_pub_sub
Definition: cost_interpretation.h:42

nav_grid_pub_sub
Author(s):
autogenerated on Sun Jan 10 2021 04:08:50