nav_grid_pub_sub: cost_interpretation

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2018, Locus Robotics
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H
00036 #define NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H
00037 
00038 #include <vector>
00039 
00040 namespace nav_grid_pub_sub
00041 {
00045 static std::vector<unsigned char> RAW(0);
00046 
00052 static std::vector<unsigned char> NEGATE =
00053 {
00054   // [0, 255] --> 255 - i
00055   255, 254, 253, 252, 251, 250, 249, 248, 247, 246,   // [0, 9]
00056   245, 244, 243, 242, 241, 240, 239, 238, 237, 236,
00057   235, 234, 233, 232, 231, 230, 229, 228, 227, 226,
00058   225, 224, 223, 222, 221, 220, 219, 218, 217, 216,
00059   215, 214, 213, 212, 211, 210, 209, 208, 207, 206,
00060   205, 204, 203, 202, 201, 200, 199, 198, 197, 196,   // [50, 59]
00061   195, 194, 193, 192, 191, 190, 189, 188, 187, 186,
00062   185, 184, 183, 182, 181, 180, 179, 178, 177, 176,
00063   175, 174, 173, 172, 171, 170, 169, 168, 167, 166,
00064   165, 164, 163, 162, 161, 160, 159, 158, 157, 156,
00065   155, 154, 153, 152, 151, 150, 149, 148, 147, 146,   // [100, 109]
00066   145, 144, 143, 142, 141, 140, 139, 138, 137, 136,
00067   135, 134, 133, 132, 131, 130, 129, 128, 127, 126,
00068   125, 124, 123, 122, 121, 120, 119, 118, 117, 116,
00069   115, 114, 113, 112, 111, 110, 109, 108, 107, 106,
00070   105, 104, 103, 102, 101, 100, 99, 98, 97, 96,       // [150, 159]
00071   95, 94, 93, 92, 91, 90, 89, 88, 87, 86,
00072   85, 84, 83, 82, 81, 80, 79, 78, 77, 76,
00073   75, 74, 73, 72, 71, 70, 69, 68, 67, 66,
00074   65, 64, 63, 62, 61, 60, 59, 58, 57, 56,
00075   55, 54, 53, 52, 51, 50, 49, 48, 47, 46,             // [200, 209]
00076   45, 44, 43, 42, 41, 40, 39, 38, 37, 36,
00077   35, 34, 33, 32, 31, 30, 29, 28, 27, 26,
00078   25, 24, 23, 22, 21, 20, 19, 18, 17, 16,
00079   15, 14, 13, 12, 11, 10, 9, 8, 7, 6,
00080   5, 4, 3, 2, 1, 0                                    // [250, 255]
00081 };
00082 
00088 std::vector<unsigned char> getOccupancyInput(bool trinary = false, bool use_unknown_value = false);
00089 
00097 static std::vector<unsigned char> OCC_GRID_PUBLISHING =
00098 {
00099   // 0: NO obstacle
00100   0,
00101   // [1, 252]: scale to [1, 98] i.e. static_cast<char>(1 + (97 * (i - 1)) / 251)
00102   1, 1, 1, 2, 2, 2, 3, 3, 4, 4,            // [1, 10]
00103   4, 5, 5, 6, 6, 6, 7, 7, 7, 8,            // [11, 20]
00104   8, 9, 9, 9, 10, 10, 11, 11, 11, 12,
00105   12, 12, 13, 13, 14, 14, 14, 15, 15, 16,
00106   16, 16, 17, 17, 18, 18, 18, 19, 19, 19,  // [41, 50]
00107   20, 20, 21, 21, 21, 22, 22, 23, 23, 23,
00108   24, 24, 24, 25, 25, 26, 26, 26, 27, 27,
00109   28, 28, 28, 29, 29, 29, 30, 30, 31, 31,
00110   31, 32, 32, 33, 33, 33, 34, 34, 35, 35,
00111   35, 36, 36, 36, 37, 37, 38, 38, 38, 39,  // [91, 100]
00112   39, 40, 40, 40, 41, 41, 41, 42, 42, 43,
00113   43, 43, 44, 44, 45, 45, 45, 46, 46, 46,
00114   47, 47, 48, 48, 48, 49, 49, 50, 50, 50,
00115   51, 51, 52, 52, 52, 53, 53, 53, 54, 54,
00116   55, 55, 55, 56, 56, 57, 57, 57, 58, 58,  // [141, 150]
00117   58, 59, 59, 60, 60, 60, 61, 61, 62, 62,
00118   62, 63, 63, 63, 64, 64, 65, 65, 65, 66,
00119   66, 67, 67, 67, 68, 68, 69, 69, 69, 70,
00120   70, 70, 71, 71, 72, 72, 72, 73, 73, 74,
00121   74, 74, 75, 75, 75, 76, 76, 77, 77, 77,  // [191, 200]
00122   78, 78, 79, 79, 79, 80, 80, 80, 81, 81,
00123   82, 82, 82, 83, 83, 84, 84, 84, 85, 85,
00124   86, 86, 86, 87, 87, 87, 88, 88, 89, 89,
00125   89, 90, 90, 91, 91, 91, 92, 92, 92, 93,
00126   93, 94, 94, 94, 95, 95, 96, 96, 96, 97,  // [241, 250]
00127   97, 98,                                  // [251, 252]
00128   // 253: inscribed obstacle
00129   99,
00130   // 254: lethal obstacle
00131   100,
00132   // 255: unknown
00133   255
00134 };
00135 
00141 std::vector<unsigned char> pixelColoringInterpretation(const double free_threshold,
00142                                                        const double occupied_threshold);
00143 
00149 std::vector<unsigned char> grayScaleInterpretation(const double free_threshold, const double occupied_threshold);
00150 
00156 static std::vector<unsigned char> TRINARY_SAVE =
00157 {
00158   // 0: NO obstacle --> white
00159   255,
00160   // [1, 253] --> gray
00161   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [1, 10]
00162   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [11, 20]
00163   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00164   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00165   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [41, 50]
00166   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00167   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00168   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00169   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00170   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [91, 100]
00171   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [101, 110]
00172   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00173   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00174   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00175   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [141, 150]
00176   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00177   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00178   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00179   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00180   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [191, 200]
00181   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00182   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00183   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00184   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00185   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [241, 250]
00186   205, 205, 205,                                     // [251, 253]
00187   // 254: --> black
00188   0,
00189   // 255 --> gray
00190   205
00191 };
00192 
00198 static std::vector<unsigned char> SCALE_SAVE =
00199 {
00200   // 0: NO obstacle --> white
00201   254,
00202   // [1, 99] --> scale from white to black
00203   252, 250, 247, 245, 242, 240, 237, 234, 232, 229,  // [1, 10]
00204   227, 224, 221, 219, 216, 214, 211, 209, 206, 203,
00205   201, 198, 196, 193, 190, 188, 185, 183, 180, 178,
00206   175, 172, 170, 167, 165, 162, 159, 157, 154, 152,
00207   149, 147, 144, 141, 139, 136, 134, 131, 129, 126,  // [41, 50]
00208   123, 121, 118, 116, 113, 110, 108, 105, 103, 100,
00209   98, 95, 92, 90, 87, 85, 82, 79, 77, 74,
00210   72, 69, 67, 64, 61, 59, 56, 54, 51, 48,
00211   46, 43, 41, 38, 36, 33, 30, 28, 25, 23,
00212   20, 17, 15, 12, 10, 7, 5, 2, 1,                    // [91, 99]
00213   // 100 --> black
00214   0,
00215   // [101, 255] --> grey
00216   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [101, 110]
00217   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00218   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00219   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00220   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [141, 150]
00221   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00222   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00223   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00224   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00225   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [191, 200]
00226   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00227   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00228   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00229   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,
00230   205, 205, 205, 205, 205, 205, 205, 205, 205, 205,  // [241, 250]
00231   205, 205, 205, 205, 205                            // [251, 255]
00232 };
00233 
00234 }  // namespace nav_grid_pub_sub
00235 
00236 #endif  // NAV_GRID_PUB_SUB_COST_INTERPRETATION_TABLES_H