43 #include <SDL/SDL_image.h> 46 #include <LinearMath/btQuaternion.h> 51 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) 58 const char* fname,
double res,
bool negate,
59 double occ_th,
double free_th,
double* origin,
64 unsigned char* pixels;
67 int rowstride, n_channels, avg_channels;
76 if(!(img = IMG_Load(fname)))
78 std::string errmsg = std::string(
"failed to open image file \"") +
79 std::string(fname) + std::string(
"\": ") + IMG_GetError();
80 throw std::runtime_error(errmsg);
84 resp->map.info.width = img->w;
85 resp->map.info.height = img->h;
86 resp->map.info.resolution = res;
87 resp->map.info.origin.position.x = *(origin);
88 resp->map.info.origin.position.y = *(origin+1);
89 resp->map.info.origin.position.z = 0.0;
92 q.setEulerZYX(*(origin+2), 0, 0);
93 resp->map.info.origin.orientation.x = q.x();
94 resp->map.info.origin.orientation.y = q.y();
95 resp->map.info.origin.orientation.z = q.z();
96 resp->map.info.origin.orientation.w = q.w();
99 resp->map.data.resize(resp->map.info.width * resp->map.info.height);
102 rowstride = img->pitch;
103 n_channels = img->format->BytesPerPixel;
107 if (mode==
TRINARY || !img->format->Amask)
108 avg_channels = n_channels;
110 avg_channels = n_channels - 1;
113 pixels = (
unsigned char*)(img->pixels);
114 for(j = 0; j < resp->map.info.height; j++)
116 for (i = 0; i < resp->map.info.width; i++)
119 p = pixels + j*rowstride + i*n_channels;
121 for(k=0;k<avg_channels;k++)
122 color_sum += *(p + (k));
123 color_avg = color_sum / (double)avg_channels;
128 alpha = *(p+n_channels-1);
131 color_avg = 255 - color_avg;
135 resp->map.data[
MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
142 occ = (255 - color_avg) / 255.0;
149 else if(occ < free_th)
151 else if(mode==
TRINARY || alpha < 1.0)
154 double ratio = (occ - free_th) / (occ_th - free_th);
158 resp->map.data[
MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
162 SDL_FreeSurface(img);
#define MAP_IDX(sx, i, j)
void loadMapFromFile(nav_msgs::GetMap::Response *resp, const char *fname, double res, bool negate, double occ_th, double free_th, double *origin, MapMode mode=TRINARY)