一、自主探索建图
自主探索建图顶层逻辑
- 找到个最佳导航点goal。认为导航到goal，能最好探索地图。
- move_base操作机器人导航到goal。
- 如果认为地图建得差不多了，结束建图，否则回到步骤1。
建图增加了两个问题。一是如何找到到最佳导航点goal，二是如何判断地图建得差不多了。针对第二个问题，往往混合在了第一个。因为找goal时，通常用迭代法，如果向迭代次数设置个阈值，超过阈值都没找到，那就认为已建出地图。
如何找到最佳导航点goal。这里说下ros官方提供的rrt_exploration功能包的两种思路。
1.1 rrt_detector
RRT：快速扩展随机树，Rapidly exploring Random Tree。
bool tnode_wrapper::exploration_detect_rrt(const costmap_2d::Costmap2D& costmap, const tpose2d& robot_pose,
geometry_msgs::Point& unk)
{
......
(init_map_x, init_map_y)是全局代价地图尺寸，是map坐标系。
bool result = false;
int rest_try = 100;
while (rest_try >= 0) {
x_rand.x = (rand() % (init_map_x - 2)) + 1;
x_rand.y = (rand() % (init_map_y - 2)) + 1;
clip(x_rand, init_map_x, init_map_y);
// Nearest
x_nearest = Nearest(V, x_rand);
clip(x_nearest, init_map_x, init_map_y);
// Steer
x_new = Steer(x_nearest, x_rand, eta);
rest_try --;
// ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle
char checking = ObstacleFree(costmap, x_nearest, x_new);x_rand是在[1, 1, init_map_x - 2, init_map_y - 2]这个矩形内产生的随机栅格。严格的话，应该要确保它不在树(V)中。要注意ros在进行全局规划时，要求goal必须位在{0, 0, size_x, size_y}矩形。
Nearset是遍历树，找到树中一个离x_rand最近结点，记为x_nearest。
Steer是此次要扩展出结点。如果x_rand离Nearset不到eta，则结果就是x_rand。否则，结果是从x_nearset到x_rand直线中的一点，距离x_nearset点eta米的位置。x_new存储着结果。
ObstacleFree。走出一条从x_nearest到xnew的直线。有三种结果。
- ObstacleFree_result_other。过程中没到遇到未知栅格或障碍栅格。
- ObstacleFree_result_obs。过程中遇到障碍栅格。
- ObstacleFree_result_unk。过程中遇到未知栅格。此时将中止走路，参数xnew会被改成未知栅格坐标。
if (checking == ObstacleFree_result_unk) {
costmap.mapToWorld(x_new.x, x_new.y, world_x, world_y);
unk.x = world_x;
unk.y = world_y;
unk.z = 0.0;
result = true;
break;过程中遇到未知栅格，xnew是那个未知栅格坐标，认为xnew就是最佳goal。找到了个goal，结束此次查找。
} else if (checking == ObstacleFree_result_other) {
V.push_back(x_new);
costmap.mapToWorld(x_new.x, x_new.y, world_x, world_y);
V_.push_back(SDL_FPoint{(float)world_x, (float)world_y});过程中没到遇到未知栅格或障碍栅格，把x_new加入到树中。
} // pub.publish(line); } return result; }
1.2 opencv_detector
这是将栅格地图进行一系列opencv操作，然后找到个最佳goal。
二、tright_exploration::updateMap逻辑
- master->updateOrigin(new_origin_x, new_origin_y)。根据此次栅格图position，更新主代价地图的map坐标系原点在world坐示系下的坐标(origin_x_, origin_y_)。
- dataptr_layer_->set_mat(info, data)。dataptr层会用此次栅格图更新私有代价地图。
- layered_costmap_->updateMap(robot_x, robot_y, robot_yaw)。更新主代价地图。
- footprint_pub_.publish(footprint)。发布此刻足迹topic。
- publisher_->publishCostmap()。发布此刻代价地图topic。
一、DataptrLayer
1.1 set_dataptr
set_dataptr作用是扣出栅格图中的3mx3m区域，“复制”到Dataptr层的私有代价地图。“复制”加引号，因为不是原则复制，要用interpretValue转换下。
void DataptrLayer::set_dataptr(const nav_msgs::MapMetaData& info, const int8_t* data)
{
resetMaps();重置整张私有价地图，方法是所有栅格赋值NO_INFORMATION(0xff)。
const Costmap2D* master = layered_costmap_->getCostmap(); int start_x; int start_y; master->worldToMapNoBounds(info.origin.position.x, info.origin.position.y, start_x, start_y);
将world坐标系下坐标info.origin.position，转换到map坐标系下坐标(start_x, start_y)，这两坐标位置关系参考图1。因为得到start_x、start_y极可能不在代价地图内，须调用忽略范围限制的worldToMapNoBounds。
int end_size_x;
int end_size_y;
int costmap_start_x = 0;
int costmap_start_y = 0;
if (start_x <= 0) {
start_x *= -1;
end_size_x = size_x_;
if (start_x + end_size_x > (int)info.width) {
// overrun border: east
end_size_x = info.width - start_x;
}
} else {
// overrun border: west
costmap_start_x = start_x;
end_size_x = size_x_ - costmap_start_x;
start_x = 0;
}在x方向，计算后面for循环要用的start_x、end_size_x、costmap_start_x。参考图1所示，计算时要考虑三处情况。
if (start_y <= 0) {
start_y *= -1;
end_size_y = size_y_;
if (start_y + end_size_y > (int)info.height) {
// overrun border: north
end_size_y = info.height - start_y;
}
} else {
// overrun border: south
costmap_start_y = start_y;
end_size_y = size_y_ - costmap_start_y;
start_y = 0;
}类似x，在y方向，计算后面for循环要用的start_y、end_size_y、costmap_start_y。
int src_index = 0;
int dst_index = 0;
// initialize the costmap with static data
for (int y = 0; y < end_size_y; ++ y) {
const int src_y_line = (y + start_y) * info.width;
const int dst_y_line = (y + costmap_start_y) * size_x_;
for (int x = 0; x < end_size_x; ++ x) {
src_index = (x + start_x) + src_y_line;
dst_index = (x + costmap_start_x) + dst_y_line;
unsigned char value = data[src_index];
costmap_[dst_index] = interpretValue(value);
}
}
}set_dataptr要调用master->worldToMapNoBounds(...)得到start_x、start_y，worldToMapNoBounds要使用主代价地图的(orign_x_, origin_y_)，所以在set_dataptr前须调用master->updateOrigin(...)，先得到正确的(orign_x_, origin_y_)。
在整个过程中，私有copstmap尺寸总是1.5mx1.5m，它的origin_x_、origin_y_没有作用，保诗着初始值(0, 0)。