(二) explore.cpp

(二) explore.cpp

使用m-explore包，这是在frontier_exploration包基础上进行的修改。可以用sudo apt install ros-${ROS_DISTRO}-explore-lite安装。需要同时运行cartographer和move_base

greedy frontier-based exploration. robot will greedily explore its environment until no frontiers could be found. 速度发送到move_base

不创建自己的costmap，节点订阅话题map（nav_msgs/OccupancyGrid类型，可配置），也可以是move_base/global_costmap/costmap。
如果是后者， 需要设置 track_unknown_space为 true

Node can do frontier filtering and can operate even on non-inflated maps. Goal blacklisting allows to deal with places inaccessible for robot

使用 move_base costmap 的好处是inflation可以处理非常小的unexplorable frontiers. 如果使用SLAM发布的raw map，应当设置 min_frontier_size to some reasonable number to deal with the small frontiers

发布话题frontiers (visualization_msgs/MarkerArray)，Each frontier is visualized by frontier points in blue and with a small sphere, which visualize the cost of the frontiers (costlier frontiers will have smaller spheres).

• progress_timeout: 时间以秒为单位。当机器人在 progress_timeout 没有任何进展时，当前目标将被放弃

• potential_scale (double, default: 1e-3): 用于加权边界。这个乘法参数影响前沿权重的前沿潜在分量（到前沿的距离） Used for weighting frontiers. This multiplicative parameter affects frontier potential component of the frontier weight (distance to frontier).

• orientation_scale (double, default: 0): 用于加权边界。这个乘法参数影响前沿权重的前沿方向分量。此参数目前不执行任何操作，仅用于向前兼容 Used for weighting frontiers. This multiplicative parameter affects frontier orientation component of the frontier weight. This parameter does currently nothing and is provided solely for forward compatibility.

• gain_scale (double, default: 1.0): 用于加权边界。这个乘法参数影响边界权重（边界大小）的边界增益分量 Used for weighting frontiers. This multiplicative parameter affects frontier gain component of the frontier weight (frontier size)

• min_frontier_size (double, default: 0.5): 将边界视为探索目标的边界的最小值 Minimum size of the frontier to consider the frontier as the exploration goal. In meters.

在Rviz使用Point按钮Prescribe一个封闭的polygon，在终端看指导

visualizeFrontiers

逻辑比较简单，关键部分如下

// weighted frontiers are always sorted
double min_cost = frontiers.empty() ? 0. : frontiers.front().cost;
  ......
m.pose.position = frontier.initial;
// scale frontier according to its cost (costier frontiers will be smaller)
// TODO: 默认的尺寸太大了，这里考虑降低 scale
double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5);

黑名单机制

主要在于变量std::vector<geometry_msgs::Point> frontier_blacklist_，由于reachedGoal被注释掉了，添加黑名单的地方只有：

if(goal_fail_flag==1)
{
  frontier_blacklist_.push_back(target_position);
  ROS_DEBUG("Adding current goal to black list");
  makePlan();
  goal_fail_flag = 0;
  return;
}

向黑名单添加目标位置，然后重新makePlan

bool Explore::goalOnBlacklist(const geometry_msgs::Point& goal)
{
  constexpr static size_t tolerace = 5;
  costmap_2d::Costmap2D* costmap2d = costmap_client_.getCostmap();
  // check if a goal is on the blacklist for goals that we're pursuing
  // 黑名单的frontier_goal 是否有一个与目前的 frontier十分接近
  for (auto& frontier_goal : frontier_blacklist_) {
    double x_diff = fabs(goal.x - frontier_goal.x);
    double y_diff = fabs(goal.y - frontier_goal.y);

if (x_diff < tolerace * costmap2d->getResolution() &&
        y_diff < tolerace * costmap2d->getResolution())
      return true;
  }
  return false;
}

在visualizeFrontiers中对接近黑名单的frontier做了红色显示

if (goalOnBlacklist(frontier.centroid)) {
  m.color = red;
} else {
  m.color = blue;
}

发布探索goal

核心的就是这几句：

// 三个设置的参数
search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(),
                                                 potential_scale_, gain_scale_,
                                                 min_frontier_size);
......
auto pose = costmap_client_.getRobotPose();
// get Frontiers sorted according to cost  返回类型std::vector<Frontier>
auto frontiers = search_.searchFrom(pose.position);

最后没有使用MoveBase的sendGoal，而是发布话题explore_goal，订阅者在manager.cpp

start和stop函数比较简单。

参考：
explore_lite
indoor_bot