Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <navfn/navfn.h>
00031 #include <stdlib.h>
00032 #include <stdint.h>
00033
00034 extern "C" {
00035 #include <stdio.h>
00036
00037 #include <pgm.h>
00038 #undef max
00039 #undef min
00040 }
00041
00042 void
00043 setcostobs(COSTTYPE *cmap, int n, int w)
00044 {
00045 int CS = 11;
00046 for (int i=-CS/2; i<CS/2; i++)
00047 {
00048 COSTTYPE *cm = i*w + &cmap[n];
00049 for (int j=-CS/2; j<CS/2; j++)
00050 cm[j] = COST_NEUTRAL + 50;
00051 }
00052 CS = 7;
00053 for (int i=-CS/2; i<CS/2; i++)
00054 {
00055 COSTTYPE *cm = i*w + &cmap[n];
00056 for (int j=-CS/2; j<CS/2; j++)
00057 cm[j] = COST_OBS;
00058 }
00059 }
00060
00061 void setcostunk(COSTTYPE *cmap, int n, int w)
00062 {
00063 cmap[n] = COST_OBS;
00064 }
00065
00066 #define unknown_gray 0xCC // seems to be the value of "unknown" in maps
00067
00068 COSTTYPE *
00069 readPGM(const char *fname, int *width, int *height, bool raw)
00070 {
00071 pm_init("navfn_tests",0);
00072
00073 FILE *pgmfile;
00074 pgmfile = fopen(fname,"r");
00075 if (!pgmfile)
00076 {
00077 printf("readPGM() Can't find file %s\n", fname);
00078 return NULL;
00079 }
00080
00081 printf("readPGM() Reading costmap file %s\n", fname);
00082 int ncols, nrows;
00083 gray maxval;
00084 int format;
00085 pgm_readpgminit(pgmfile, &ncols, &nrows, &maxval, &format);
00086 printf("readPGM() Size: %d x %d\n", ncols, nrows);
00087
00088
00089 COSTTYPE *cmap = (COSTTYPE *)malloc(ncols*nrows*sizeof(COSTTYPE));
00090 if (!raw)
00091 for (int i=0; i<ncols*nrows; i++)
00092 cmap[i] = COST_NEUTRAL;
00093
00094 gray * row(pgm_allocrow(ncols));
00095 int otot = 0;
00096 int utot = 0;
00097 int ftot = 0;
00098 for (int ii = 0; ii < nrows; ii++) {
00099 pgm_readpgmrow(pgmfile, row, ncols, maxval, format);
00100 if (raw)
00101 {
00102 for (int jj(ncols - 1); jj >= 0; --jj)
00103 {
00104 int v = row[jj];
00105 cmap[ii*ncols+jj] = v;
00106 if (v >= COST_OBS_ROS)
00107 otot++;
00108 if (v == 0)
00109 ftot++;
00110 }
00111 }
00112 else
00113 {
00114 ftot = ncols*nrows;
00115 for (int jj(ncols - 1); jj >= 0; --jj)
00116 {
00117 if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7)
00118 {
00119 setcostobs(cmap,ii*ncols+jj,ncols);
00120 otot++;
00121 ftot--;
00122 }
00123 else if (row[jj] <= unknown_gray)
00124 {
00125 setcostunk(cmap,ii*ncols+jj,ncols);
00126 utot++;
00127 ftot--;
00128 }
00129 }
00130 }
00131 }
00132 printf("readPGM() Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot);
00133 pgm_freerow(row);
00134 *width = ncols;
00135 *height = nrows;
00136 return cmap;
00137 }