Map CreateMap(ConfigurationManager config)
{
std::vector<unsigned char> image;
unsigned width, height;
unsigned int x, y;
const char* path = config.getMapPath();
unsigned error = lodepng::decode(image, width, height, path);
if (error)
{
std::cout << "decoder error " << error << ": "
<< lodepng_error_text(error) << std::endl;
}
// Get parameters
double gridResoultion = config.getGridResolutionCM();
double mapResoultion = config.getMapResolutionCM();
int divider = ceil(gridResoultion/mapResoultion); // Numbers of pixels for 1 cell in matrix
// Create map as png pixels
char** map = initializeMatrix(height + (divider - 1) * 2,width + (divider - 1) * 2, FREE_CELL);
unsigned char color;
for (y = 0; y < height; y++)
{
for (x = 0; x < width; x++)
{
if (image[y * width * 4 + x * 4 + 0]
|| image[y * width * 4 + x * 4 + 1]
|| image[y * width * 4 + x * 4 + 2])
{
// There is an obstacle in the map
color = FREE_CELL;
}
else
{
// There is no obstacle
color = BLOCK_CELL;
}
map[y + divider - 1][x + divider - 1] = color;
}
}
// Create map as config cm size
int newMapHieght = height/divider;
int newMapWidth = width/divider;
char** mapBlow = initializeMatrix(newMapHieght, newMapWidth, FREE_CELL);
unsigned i,j;
int mapBlowY = 0, mapBlowX = 0;
bool hasObstacle = false;
for (y = divider - 1; y < height; y += divider)
{
for (x = divider - 1; x < width; x += divider)
{
for (i = 0; i < divider && !hasObstacle; i++)
{
for (j = 0; j < divider && !hasObstacle; j++)
{
if (map[y + i][x + j] == BLOCK_CELL)
{
hasObstacle = true;
}
}
}
if (hasObstacle)
{
mapBlow[mapBlowY][mapBlowX] = BLOCK_CELL;
}
hasObstacle = false;
mapBlowX++;
}
mapBlowX = 0;
mapBlowY++;
}
// ReSize obstacle
double* robotSize = config.getRobotSize();
int sizeH = ceil(((robotSize[0] / gridResoultion)- 1) / 2);
int sizeW = ceil(((robotSize[1] / gridResoultion)- 1) / 2);
char** newMap = initializeMatrix(newMapHieght + sizeH*2, newMapWidth + sizeW*2, FREE_CELL);
for (int i = 0; i < newMapHieght; i++)
{
for (int j = 0; j < newMapWidth; j++)
{
if (mapBlow[i][j] == BLOCK_CELL)
{
for (int iNewMap = i; iNewMap <= i + sizeH + 1; iNewMap++)
{
for (int jNewMap = j; jNewMap <= j + sizeW + 1; jNewMap++)
{
newMap[iNewMap][jNewMap] = BLOCK_CELL;
}
}
}
}
}
// Initialize final map with wall block of size 1.
char** finalMap = initializeMatrix(newMapHieght + 2, newMapWidth + 2, BLOCK_CELL);
for (int i=1; i < newMapHieght + 1; i++)
{
for (int j=1; j< newMapWidth + 1; j++)
{
finalMap[i][j] = newMap[sizeH + i - 1][sizeW + j -1];
}
}
printMap(map, height + 6, width + 6);
printMap(mapBlow, newMapHieght, newMapWidth);
printMap(newMap, newMapHieght + sizeH, newMapWidth + sizeW);
printMap(finalMap, newMapHieght + 2, newMapWidth + 2);
return Map(finalMap, newMapHieght + 2, newMapWidth + 2, gridResoultion / 100);
}
void Map::initMap(const char* filename) {
std::vector<unsigned char> image;
std::vector<unsigned char> FatImage;
unsigned width, height;
unsigned x, y, bX, bY;
// Get the config
ConfigurationManager* config = ConfigurationManager::GetInstance();
//decode
unsigned error = lodepng::decode(image, width, height,
config->getPngMapPath());
//if there's an error, display it
if (error)
std::cout << "decoder error " << error << ": "
<< lodepng_error_text(error) << std::endl;
// Create the fat img
FatImage.resize(width * height);
// calc the size of the robot in pic px
unsigned PxToBlow = ceil(
config->getRobotSize().RadiosSize()
/ config->getPngGridResolution());
// run on the map and find the
unsigned char color;
for (y = 0; y < height; y++)
for (x = 0; x < width; x++) {
if (image[y * width * 4 + x * 4 + 0]
|| image[y * width * 4 + x * 4 + 1]
|| image[y * width * 4 + x * 4 + 2])
color = 0;
// add oppstical to the fat img
for (bX = std::max(x - PxToBlow, static_cast<unsigned int>(0));
bX < PxToBlow + x; bX++)
for (bY = std::max(x - PxToBlow, static_cast<unsigned int>(0));
bY < PxToBlow + y; bY++) {
FatImage[y * width * 4 + x * 4 + 0] = color;
FatImage[y * width * 4 + x * 4 + 1] = color;
FatImage[y * width * 4 + x * 4 + 2] = color;
FatImage[y * width * 4 + x * 4 + 3] = 255;
}
}
// create grid from the fat and regular map
this->FatGrid = this->CreatGridFromMap(FatImage, height, width,
config->getPngGridResolution(), config->getPixelPerCm(),
this->m_Cols, this->m_Rows);
unsigned error2 = lodepng::encode("try.png", this->FatGrid, width, height);
this->RegGrid = this->CreatGridFromMap(image, height, width,
config->getPngGridResolution(), config->getPixelPerCm(),
this->m_Cols, this->m_Rows);
}
