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 MapMode mode)
00059 {
00060 SDL_Surface* img;
00061
00062 unsigned char* pixels;
00063 unsigned char* p;
00064 unsigned char value;
00065 int rowstride, n_channels, avg_channels;
00066 unsigned int i,j;
00067 int k;
00068 double occ;
00069 int alpha;
00070 int color_sum;
00071 double color_avg;
00072
00073
00074 if(!(img = IMG_Load(fname)))
00075 {
00076 std::string errmsg = std::string("failed to open image file \"") +
00077 std::string(fname) + std::string("\"");
00078 throw std::runtime_error(errmsg);
00079 }
00080
00081
00082 resp->map.info.width = img->w;
00083 resp->map.info.height = img->h;
00084 resp->map.info.resolution = res;
00085 resp->map.info.origin.position.x = *(origin);
00086 resp->map.info.origin.position.y = *(origin+1);
00087 resp->map.info.origin.position.z = 0.0;
00088 tf::Quaternion q;
00089 q.setRPY(0,0, *(origin+2));
00090 resp->map.info.origin.orientation.x = q.x();
00091 resp->map.info.origin.orientation.y = q.y();
00092 resp->map.info.origin.orientation.z = q.z();
00093 resp->map.info.origin.orientation.w = q.w();
00094
00095
00096 resp->map.data.resize(resp->map.info.width * resp->map.info.height);
00097
00098
00099 rowstride = img->pitch;
00100 n_channels = img->format->BytesPerPixel;
00101
00102
00103
00104 if (mode==TRINARY || !img->format->Amask)
00105 avg_channels = n_channels;
00106 else
00107 avg_channels = n_channels - 1;
00108
00109
00110 pixels = (unsigned char*)(img->pixels);
00111 for(j = 0; j < resp->map.info.height; j++)
00112 {
00113 for (i = 0; i < resp->map.info.width; i++)
00114 {
00115
00116 p = pixels + j*rowstride + i*n_channels;
00117 color_sum = 0;
00118 for(k=0;k<avg_channels;k++)
00119 color_sum += *(p + (k));
00120 color_avg = color_sum / (double)avg_channels;
00121
00122 if (n_channels == 1)
00123 alpha = 1;
00124 else
00125 alpha = *(p+n_channels-1);
00126
00127 if(negate)
00128 color_avg = 255 - color_avg;
00129
00130 if(mode==RAW){
00131 value = color_avg;
00132 resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
00133 continue;
00134 }
00135
00136
00137
00138
00139 occ = (255 - color_avg) / 255.0;
00140
00141
00142
00143
00144 if(occ > occ_th)
00145 value = +100;
00146 else if(occ < free_th)
00147 value = 0;
00148 else if(mode==TRINARY || alpha < 1.0)
00149 value = -1;
00150 else {
00151 double ratio = (occ - free_th) / (occ_th - free_th);
00152 value = 99 * ratio;
00153 }
00154
00155 resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
00156 }
00157 }
00158
00159 SDL_FreeSurface(img);
00160 }
00161
00162 }