C++ ConfigurationManager::getRobotSize方法代码示例

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;
					}
				}
			}
//.........这里部分代码省略.........

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);

}