read_pgm_costmap.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2012, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <navfn/navfn.h>
31 #include <stdlib.h>
32 #include <stdint.h>
33 
34 #ifdef __APPLE__
35 # include <netpbm/pgm.h>
36 #else
37 extern "C" {
38 #include <stdio.h>
39 // pgm.h is not very friendly with system headers... need to undef max() and min() afterwards
40 #include <pgm.h>
41 #undef max
42 #undef min
43 }
44 #endif
45 
46 void
47 setcostobs(COSTTYPE *cmap, int n, int w)
48 {
49  int CS = 11;
50  for (int i=-CS/2; i<CS/2; i++)
51  {
52  COSTTYPE *cm = i*w + &cmap[n];
53  for (int j=-CS/2; j<CS/2; j++)
54  cm[j] = COST_NEUTRAL + 50;
55  }
56  CS = 7;
57  for (int i=-CS/2; i<CS/2; i++)
58  {
59  COSTTYPE *cm = i*w + &cmap[n];
60  for (int j=-CS/2; j<CS/2; j++)
61  cm[j] = COST_OBS;
62  }
63 }
64 
65 void setcostunk(COSTTYPE *cmap, int n, int w)
66 {
67  cmap[n] = COST_OBS;
68 }
69 
70 #define unknown_gray 0xCC // seems to be the value of "unknown" in maps
71 
72 COSTTYPE *
73 readPGM(const char *fname, int *width, int *height, bool raw)
74 {
75  pm_init("navfn_tests",0);
76 
77  FILE *pgmfile;
78  pgmfile = fopen(fname,"r");
79  if (!pgmfile)
80  {
81  printf("readPGM() Can't find file %s\n", fname);
82  return NULL;
83  }
84 
85  printf("readPGM() Reading costmap file %s\n", fname);
86  int ncols, nrows;
87  gray maxval;
88  int format;
89  pgm_readpgminit(pgmfile, &ncols, &nrows, &maxval, &format);
90  printf("readPGM() Size: %d x %d\n", ncols, nrows);
91 
92  // set up cost map
93  COSTTYPE *cmap = (COSTTYPE *)malloc(ncols*nrows*sizeof(COSTTYPE));
94  if (!raw)
95  for (int i=0; i<ncols*nrows; i++)
96  cmap[i] = COST_NEUTRAL;
97 
98  gray * row(pgm_allocrow(ncols));
99  int otot = 0;
100  int utot = 0;
101  int ftot = 0;
102  for (int ii = 0; ii < nrows; ii++) {
103  pgm_readpgmrow(pgmfile, row, ncols, maxval, format);
104  if (raw) // raw costmap from ROS
105  {
106  for (int jj(ncols - 1); jj >= 0; --jj)
107  {
108  int v = row[jj];
109  cmap[ii*ncols+jj] = v;
110  if (v >= COST_OBS_ROS)
111  otot++;
112  if (v == 0)
113  ftot++;
114  }
115  }
116  else
117  {
118  ftot = ncols*nrows;
119  for (int jj(ncols - 1); jj >= 0; --jj)
120  {
121  if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7)
122  {
123  setcostobs(cmap,ii*ncols+jj,ncols);
124  otot++;
125  ftot--;
126  }
127  else if (row[jj] <= unknown_gray)
128  {
129  setcostunk(cmap,ii*ncols+jj,ncols);
130  utot++;
131  ftot--;
132  }
133  }
134  }
135  }
136  printf("readPGM() Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot);
137  pgm_freerow(row);
138  *width = ncols;
139  *height = nrows;
140  return cmap;
141 }
void setcostunk(COSTTYPE *cmap, int n, int w)
COSTTYPE * readPGM(const char *fname, int *width, int *height, bool raw)
void setcostobs(COSTTYPE *cmap, int n, int w)
#define unknown_gray


navfn
Author(s): Kurt Konolige, Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:40