【自主探索】frontier_exploration 源码解析

各文件运行顺序：

1. \exploration_server\launch\exploration.launch
2. \exploration_server\src\plugin_client.cpp
3. \exploration_server\src\exploration_server_node.cpp
4. \exploration_server\src\exploration_server.cpp
5. \frontier_exploration\src\frontier_plugin.cpp
6. \frontier_exploration\src\frontier_search.cpp

可以先看 explore_lite 包的源码，是根据这个包优化的，关键代码都是相同的，但由于 explore_lite 删减了用户初始自定义探索区域的操作，个人感觉结构更加清晰。
【自主探索】explore_lite 源码解析
frontier_exploration 和 explore_lite 源码中相同的部分本篇没有另外解析。

一、exploration.launch

<!-- 这是一个ROS的Launch文件，
用于启动 exploration_server 包中的 exploration_server_node 节点和 plugin_client 节点，该节点的目的是执行探索任务，
使用 polygon_layer 进行地图标记，并通过 sensor 获取激光传感器数据。 -->

<launch>
    <arg name="plugin" default="exploration_server::ExamplePlugin"/>
    <param name="plugin_name" value="$(arg plugin)"/>
    <arg name="blacklist_box_size" default="0.5"/>
    <param name="blacklist_box_size" value="$(arg blacklist_box_size)"/>

    <!-- Set to your sensor's range 用于设置传感器的范围-->
    <arg name="sensor_range" default="1.0"/>

    <!-- 该节点负责与插件进行通信。 -->
    <node pkg="exploration_server" type="plugin_client" name="plugin_client" output="screen"/>  

    <!-- 该节点是执行探索任务的主要节点 -->
    <node pkg="exploration_server" type="exploration_server_node" name="exploration_server_node" output="screen">

        <rosparam param="points" ns="explore_costmap" file="$(find exploration_server)/config/points.yaml" command="load"/>
        <param name="frequency" type="double" value="2.0"/>
        <param name="goal_aliasing" type="double" value="$(arg sensor_range)"/>

        #All standard costmap_2d parameters as in move_base, other than PolygonLayer
        <rosparam ns="explore_costmap" subst_value="true">
            footprint: [[0.1, 0.0], [0.0, 0.1], [-0.1, 0.0], [0.0, -0.1]]
            robot_radius: 0.10

            transform_tolerance: 0.5
            update_frequency: 5.0
            publish_frequency: 5.0

            #must match incoming static map
            global_frame: map
            robot_base_frame: base_link
            resolution: 0.05

            rolling_window: false
            track_unknown_space: true

            plugins:

                - {
   name: static,           type: "costmap_2d::StaticLayer"}
                - {
   name: polygon_layer,    type: "polygon_layer::PolygonLayer"}
                #Can disable sensor layer if gmapping is fast enough to update scans
                - {
   name: sensor,           type: "costmap_2d::ObstacleLayer"}
                - {
   name: inflation,        type: "costmap_2d::InflationLayer"}

            static:
                #Can pull data from gmapping, map_server or a non-rolling costmap
                map_topic: /map
                # map_topic: move_base/global_costmap/costmap
                subscribe_to_updates: true

            polygon_layer:
                resize_to_polygon: true

            sensor:
                observation_sources: laser
                laser: {
   data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true, raytrace_range: $(arg sensor_range), obstacle_range: $(arg sensor_range)}

            inflation:
                inflation_radius: 0.15
      </rosparam>

    </node>

</launch>

二、plugin_client.cpp

#include <exploration_server/plugin_client.h>
#include <actionlib/client/simple_action_client.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <exploration_server/geometry_tools.h>

#include <exploration_msgs/ExploreAction.h>
#include <visualization_msgs/Marker.h>
#include <string>

int main(int argc, char** argv)
{
   
    ros::init(argc, argv, "plugin_client");

    exploration_server::PluginClient client;
    ros::spin();
    return 0;
}

namespace exploration_server
{
   
	......

}  // namespace exploration_server

    // 1.
  PluginClient::PluginClient() :
      nh_(),    //nh_ 是一个 ROS 节点句柄，用于与 ROS 系统进行通信
      private_nh_("~"), //private_nh_ 是 nh_ 的私有句柄，用于获取节点的私有参数
      waiting_for_center_(false)    //初始化 waiting_for_center_ 为 false，该标志用于指示是否正在等待多边形的中心点
  {
   
      //从 ROS 参数服务器中获取名为 "plugin_name" 的参数值，如果不存在则使用默认值 "exploration_server::ExamplePlugin"。这个参数表示要使用的插件的名称
      nh_.param<std::string>("plugin_name", plugin_name_, "exploration_server::ExamplePlugin");
      input_.header.frame_id = "map";   //设置输入消息 input_ 的帧 ID 为 "map"
      // 创建一个订阅器，订阅 "/clicked_point" 话题，消息类型为 geometry_msgs::PointStamped。
      // 当有新消息到达时，将调用 &PluginClient::pointCb 函数处理，函数的上下文是当前对象 (this)。
      point_ = nh_.subscribe("/clicked_point", 10, &PluginClient::pointCb, this);
      //创建一个发布者，发布类型为 visualization_msgs::Marker 的消息到 "exploration_polygon_marker" 话题。该发布者用于发布多边形的可视化标记。
      point_viz_pub_ = nh_.advertise<visualization_msgs::Marker>("exploration_polygon_marker", 10);
      //创建一个定时器，定时每隔 0.1 秒调用 &PluginClient::vizPubCb 函数。这个函数用于定时发布多边形的可视化标记。
      point_viz_timer_ = nh_.createWallTimer(ros::WallDuration(0.1), boost::bind(&PluginClient::vizPubCb, this));
      //通过 ROS_INFO 输出一条信息，提示用户在 RViz 中使用 'Point' 工具选择一个勘探边界。
      ROS_INFO("Please use the 'Point' tool in Rviz to select an exporation boundary.");
  }

    // 2.
  void PluginClient::pointCb(const geometry_msgs::PointStampedConstPtr& point)
  {
   
      // 计算输入多边形的平均边长，用于后续检查控制点的距离。
      double average_distance = polygonPerimeter(input_.polygon) / input_.polygon.points.size();

      if (waiting_for_center_)  //如果正在等待中心点，表示这是边界多边形的最后一个点，即中心点
      {
   
          // flag is set so this is the last point of boundary polygon, i.e. center
            // 检查中心点是否在多边形内，如果不在则输出错误信息
          if (!pointInPolygon(point->point, input_.polygon))
          {
   
              ROS_ERROR("Center not inside polygon, restarting");
          }
          else
          {
   
                // 创建 exploreClient，用于与 exploration_server 节点通信
              actionlib::SimpleActionClient<exploration_msgs::ExploreAction>
              exploreClient("exploration_server_node", true);
              exploreClient.waitForServer();
              ROS_INFO("Sending goal");
              // 创建并发送勘探目标到 exploration_server
              exploration_msgs::ExploreGoal goal;
              goal.start_point = *point;
              goal.boundary = input_;
              // send the name of the plugin you want to use to get goals   发送用于获取目标的插件名称
              goal.strategy_plugin = plugin_name_;
              exploreClient.sendGoal(goal);
          }
          // 将 waiting_for_center_ 标志设为 false，清空输入多边形的点集
          waiting_for_center_ = false;
          input_.polygon.points.clear();
      }
      else if (input_.polygon.points.empty())
      {
   
          // first control point, so initialize header of boundary polygon
            // 如果输入多边形的点集为空，表示这是第一个控制点，初始化多边形头部信息
          input_.header = point->header;
          input_.polygon.points.push_back(costmap_2d::toPoint32(point->point));
      }
      else if (input_.header.frame_id != point->header.frame_id)
      {
   
            // 如果输入多边形的帧ID与当前点的帧ID不匹配，输出错误信息并重置多边形的点集
          ROS_ERROR("Frame mismatch, restarting polygon selection");
          input_.polygon.points.clear();
      }
      // 如果多边形的点集中有至少两个点，并且当前点与第一个点的距离在平均边长的 0.1 倍以内，表示这是最后一个边界点，即附近于第一个点
      else if (input_.polygon.points.size() > 1 &&
        pointsNearby