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
00031
00032
00033
00034
00035
00036 #include <cstring>
00037 #include <stdexcept>
00038
00039 #include <stdlib.h>
00040 #include <stdio.h>
00041
00042
00043 #include <SDL/SDL_image.h>
00044
00045 #include "map_server/image_loader.h"
00046 #include <tf/tf.h>
00047
00048
00049 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00050
00051 namespace map_server
00052 {
00053
00054 void
00055 loadMapFromFile(nav_msgs::GetMap::Response* resp,
00056 const char* fname, double res, bool negate,
00057 double occ_th, double free_th, double* origin)
00058 {
00059 SDL_Surface* img;
00060
00061 unsigned char* pixels;
00062 unsigned char* p;
00063 int rowstride, n_channels;
00064 unsigned int i,j;
00065 int k;
00066 double occ;
00067 int color_sum;
00068 double color_avg;
00069
00070
00071 if(!(img = IMG_Load(fname)))
00072 {
00073 std::string errmsg = std::string("failed to open image file \"") +
00074 std::string(fname) + std::string("\"");
00075 throw std::runtime_error(errmsg);
00076 }
00077
00078
00079 resp->map.info.width = img->w;
00080 resp->map.info.height = img->h;
00081 resp->map.info.resolution = res;
00082 resp->map.info.origin.position.x = *(origin);
00083 resp->map.info.origin.position.y = *(origin+1);
00084 resp->map.info.origin.position.z = 0.0;
00085 tf::Quaternion q;
00086 q.setRPY(0,0, *(origin+2));
00087 resp->map.info.origin.orientation.x = q.x();
00088 resp->map.info.origin.orientation.y = q.y();
00089 resp->map.info.origin.orientation.z = q.z();
00090 resp->map.info.origin.orientation.w = q.w();
00091
00092
00093 resp->map.data.resize(resp->map.info.width * resp->map.info.height);
00094
00095
00096 rowstride = img->pitch;
00097 n_channels = img->format->BytesPerPixel;
00098
00099
00100 pixels = (unsigned char*)(img->pixels);
00101 for(j = 0; j < resp->map.info.height; j++)
00102 {
00103 for (i = 0; i < resp->map.info.width; i++)
00104 {
00105
00106 p = pixels + j*rowstride + i*n_channels;
00107 color_sum = 0;
00108 for(k=0;k<n_channels;k++)
00109 color_sum += *(p + (k));
00110 color_avg = color_sum / (double)n_channels;
00111
00112
00113
00114 if(negate)
00115 occ = color_avg / 255.0;
00116 else
00117 occ = (255 - color_avg) / 255.0;
00118
00119
00120
00121
00122 if(occ > occ_th)
00123 resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = +100;
00124 else if(occ < free_th)
00125 resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
00126 else
00127 resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
00128 }
00129 }
00130
00131 SDL_FreeSurface(img);
00132 }
00133
00134 }