显示地图(类似rviz)(源码注释)

• rose地图、ros地图。“rose地图”专门指场景编程中的地图，“ros地图”则指Ros订阅到的地图。
• raw-rose地图。指栅格尺寸等于1/resolution时的rose地图。举个例子，resolution是0.05时，raw-rose地图就是栅格尺寸等于20时的rose地图。
• rose地图中的一个栅格总是表示一米，
• ros地图放置到raw-rose地图，放置规则有两条。1）以双方的左上角对齐。2）ros地图的map坐标系原点必须放置在raw_rose地图栅格的交叉点。可这样理解放置过程：底下是静止的raw-rose地图，上面是可移动的ros地图，两个地图首先左上角对齐，然后ros地图向右移动，让map坐标系原点的x落在距离最近的两个栅格交叉点；对齐x后，ros地图向下移动，让map坐标系原点的y落在距离最近两个栅格交叉点。移动过程中，rose地图交叉点处栅格的loc_.x、loc_.y不必是UNIT_LOCS整数倍。

一、几个变量

1.1 map_controller

1.2 unit

二、unit::app_draw_unit

app_draw_unit是base_unit虚函数，通过重载它，launcher画出各个单元应该的图像。

void unit::app_draw_unit(const int xsrc, const int ysrc)
{
	const cv::Mat& full = controller_.last_src_gray();
	if (full.rows == 0 || full.cols == 0) {
		return;
	}

	// UNIT_LOCS值是2，表示水平、垂直方向各2个栅格组成一个单元。
	const int unit_size = disp_.zoom() * UNIT_LOCS;
	const SDL_Point& map_offset = controller_.last_map_offset();

	const nav_msgs::MapMetaData& last_map = controller_.last_map();
	// 为做到精确，此处要保证“1能被map.info.resolution整除”，这样算出的simple_per_meter不含小数。
	const int simple_per_meter = 1 / last_map.resolution;
	VALIDATE(simple_per_meter == 20, null_str);
	const int simple_per_meters = simple_per_meter * UNIT_LOCS;

	// step1：计算图1斜线标识出的roi，但以Om为原点的矩形。即loc{1,0}的roi.x、roi.y是map_offset。
	// {mapx, mapy, w, h}是该单元在raw-rose地图中的矩形。
	const int mapx = loc_.x * simple_per_meter;
	const int mapy = loc_.y * simple_per_meter;
	const int w = simple_per_meters;
	const int h = simple_per_meters;

	if (mapx + w <= map_offset.x || mapx >= map_offset.x + full.cols) {
		return;
	}
	if (mapy + h <= map_offset.y || mapy >= map_offset.y + full.rows) {
		return;
	}

	cv::Rect roi(mapx, mapy, w, h);
	if (roi.x < map_offset.x) {
		roi.width -= map_offset.x - roi.x;
		roi.x = map_offset.x;
	}

	if (roi.y < map_offset.y) {
		roi.height -= map_offset.y - roi.y;
		roi.y = map_offset.y;
	}

	if (roi.x + roi.width > map_offset.x + full.cols) {
		roi.width -= roi.x + roi.width - (map_offset.x + full.cols);
	}

	if (roi.y + roi.height > map_offset.y + full.rows) {
		roi.height -= roi.y + roi.height - (map_offset.y + full.rows);
	}

	VALIDATE(roi.x >= map_offset.x, null_str);
	VALIDATE(roi.y >= map_offset.y, null_str);

	// 图2显示了x、y，它是屏幕坐标系。该单元中的ros地图图像从此坐标开始渲染。
	int x = xsrc + (roi.x - mapx) * unit_size / simple_per_meters;
	int y = ysrc + (roi.y - mapy) * unit_size / simple_per_meters;

	// 图2显示了not_draw、draw_width。
	// 要是不做去noise，可能会导致单元右侧或下侧出现1像素空隙
	int draw_width = roi.width * unit_size / simple_per_meters;
	if (roi.x - mapx != 0) {
		const int gap = xsrc % unit_size;
		const int not_draw = (x - gap) % unit_size;
		int noise = (unit_size - not_draw) - draw_width;
		x += noise;
	}
	int draw_height = roi.height * unit_size / simple_per_meters;
	if (roi.y - mapy != 0) {
		const int gap = ysrc % unit_size;
		const int not_draw = (y - gap) % unit_size;
		int noise = (unit_size - not_draw) - draw_height;
		y += noise;
	}

	// step2：roi.x、roi.y归0化，即x、y减去map_offset。
	roi.x -= map_offset.x;
	roi.y -= map_offset.y;

	// 按实际栅格尺寸，放大roi部分的图像
	cv::Size cvSize(draw_width, draw_height);
	cv::Mat zoomed_gray, argb;
	cv::resize(full(roi), zoomed_gray, cvSize);
	cv::cvtColor(zoomed_gray, argb, cv::COLOR_GRAY2BGRA);

	surface surf(argb);
	disp_.drawing_buffer_add(display::LAYER_UNIT_DEFAULT, loc_, surf, x, y, 0, 0);

	// step3：绘制map坐标系原点
	const SDL_Point& map_origin = controller_.last_map_origin();
  	// ros坐标系-->raw-ros坐标系。offsetx/offset.y是map坐标系原点在raw-ros坐标系中的坐标。
	int offsetx = map_offset.x + map_origin.x;
	int offsety = map_offset.y + map_origin.y;
	if (point_in_rect(offsetx, offsety, SDL_Rect{mapx, mapy, w, h})) {
		surf = image::get_image("misc/map_origin.png");
		int deltax = (offsetx - mapx) * unit_size / simple_per_meters - surf->w / 2;
		int deltay = (offsety - mapy) * unit_size / simple_per_meters - surf->h / 2;
		disp_.drawing_buffer_add(display::LAYER_UNIT_DEFAULT, loc_, surf,
			xsrc + deltax, ysrc + deltay,
			0, 0);
	}

	// step4：绘制laser坐标系原点及位姿
	const SDL_Point& laser_origin = controller_.last_laser_origin();
	offsetx = map_offset.x + laser_origin.x;
	offsety = map_offset.y + laser_origin.y;
	if (point_in_rect(offsetx, offsety, SDL_Rect{mapx, mapy, w, h})) {
		surf = controller_.laser_png_surf();
		const SDL_Point& rotate_point = controller_.laser_png_rotate_point();
		int deltax = (offsetx - mapx) * unit_size / simple_per_meters - rotate_point.x;
		int deltay = (offsety - mapy) * unit_size / simple_per_meters - rotate_point.y;
		// laser.png overlay map_origin.png always. use higher layer.
		disp_.drawing_buffer_add(display::LAYER_UNIT_MOVE_DEFAULT, loc_, surf,
			xsrc + deltax, ysrc + deltay,
			0, 0);
	}
}

simple_per_meter是每米多少个数素，像resolution是0.05时，值是20。由于一个栅格总是表示一米，simple_per_meters则表示raw-rose地图中每单元表示的像素数。

roi(Range Of Interest)指此次某单元要渲染的图像区域，见图1中的斜线标示部分。全是在raw-rose地图计算roi。分两步计算。以loc_{1,0}栅格为列

1. 计算出图斜线标示部分，但是以Om为原点的矩形。即loc_{1,0}的roi中x、y是map_offset。
2. roi的x、y归0化，即x、y减去map_offset。