一、概述

嘗試 frontier_exploration 的最佳方法是使用 husky_navigation 中提供的演示，請參閱演示教程。

1、功能

frontier_exploration 軟件包提供了 costmap_2d 層插件 BoundedExploreLayer 以及 actionlib 客戶端/服務器節點 explore_client 和 explore_server。

所提供的節點可用于演示成本圖層的功能，方法是執行一個以用戶定義的多邊形區域為邊界的邊界探索任務。

BoundedExploreLayer 圖層當然也可用于執行更復雜的勘探任務，其功能通過兩個服務實現： UpdatePolygonBoundary 和 GetNextFrontier。

2、要求

使用本軟件包進行邊界探索，需要一個真實或模擬的機器人配置，以提供以下功能：

1. 激光掃描儀或類似傳感器，可清理空間并標記障礙物。
2. 適當配置的導航堆棧，可接受移動基地的行動目標。
3. (可選）由 map_server、gmapping 或 move_base 提供的全局 / 地圖

二、使用方法

1、用于運行演示

使用預先存在的機器人和配置，您可以運行一個演示，看看該軟件包如何工作。

1. 安裝 frontier_exploration 算法

sudo apt-get install ros-kinetic-frontier-exploration ros-kinetic-navigation-stage

2. 在獨立終端逐一運行必要的節點：

roslaunch navigation_stage move_base_gmapping_5cm.launch
roslaunch navigation_stage move_base.xml
roslaunch frontier_exploration global_map.launch

彈出 RViz，然后在地圖中心周圍出現一個演示機器人。

3. 在 RViz 上打開 Marker 插件（RViz 插件可在彈出窗口中選擇，通過 "添加 "按鈕打開）。
4. 下拉 "Displays --> Marker --> Marker Topic "菜單，然后選擇 "exploration_polygon_marker "主題。
5. 在 RViz 的地圖上，想一個您希望機器人探索的區域。
6. 點擊 RViz 頂部的 “Publish Point”。
7. 點擊該區域 n 個角中的一個角（如果您的區域是正方形/長方形，請點擊 n=4 個角）。
8. 重復上述第 6 和第 7 步 n 次。之后你會看到一個有 n 個角的多邊形。
9. 再次執行步驟 6，然后點擊多邊形內的任意位置。

視頻演示

看到有個博主跑這個包沒成功，出現報錯

ERROR: cannot launch node of type [frontier_exploration/explore_client]: Cannot locate node of type [explore_client] in package [frontier_exploration]. Make sure file exists in package path and permission is set to executable (chmod +x)
ERROR: cannot launch node of type [frontier_exploration/explore_server]: Cannot locate node of type [explore_server] in package [frontier_exploration]. Make sure file exists in package path and permission is set to executable (chmod +x)

評論區說編譯通過以后在/catkin_ws/devel/lib/frontier_exploration下沒有任何文件，所以會出現此報錯，感覺包本身是有問題的

2、用于開發人員

如果你只是想開始利用這個軟件包的功能，husky_navigation 軟件包中的教程會有所幫助。

如果你想了解得更深入一些，一般來說，當啟動 explore_server 時，它會一直旋轉，直到收到一個探索目標。要提交目標：

1. 使用 explore_client 和 RViz - 在 Rviz 中為 exploration_polygon_marker 主題創建一個標記顯示，并使用工具欄上的 "Click Point "工具標記探索邊界。留意 ROS 控制臺對所選邊界的反饋。
2. 通過 actionlib SimpleActionClient，用自己的節點向服務器提交目標。

探索目標包含一個開始探索的初始點和一個限制探索范圍的多邊形邊界。要運行無邊界探索任務，只需將邊界留空即可。

服務器收到目標后，就會創建初始探索地圖，開始處理傳感器/成本地圖數據，并發出 move_base 行動目標。默認情況下，探索任務將探索邊界內的所有區域（無論之前是否訪問過）。下面提供了幾種使用案例的啟動文件示例。

2.1. 探索無/地圖數據

在沒有全局/地圖信息源的情況下運行動作服務器/客戶端時，請啟用 resize_too_boundary 參數，以便根據動作目標的多邊形邊界動態調整地圖大小。當機器人在探索邊界外行進時，costmap_2d 會出現傳感器超出地圖邊界的錯誤信息。這些信息可以安全地忽略，也可以使用 rosconsole 配置文件加以抑制。

如果不使用 resize_to_boundary（例如運行無邊界探索），請確保成本地圖配置了足夠大的高度/寬度。

啟動文件示例：no_global_map.launch

<launch><!-- Set to your sensor's range --><arg name="sensor_range" default="1.0"/><node pkg="frontier_exploration" type="explore_client" name="explore_client" output="screen"/><node pkg="frontier_exploration" type="explore_server" name="explore_server" output="screen" ><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 BoundedExploreLayer<rosparam ns="explore_costmap" subst_value="true">#Sample parametersfootprint: [[0.1, 0.0], [0.0, 0.1], [0.0, -0.1], [-0.1, 0.0]]robot_radius: 0.10transform_tolerance: 0.5update_frequency: 5.0publish_frequency: 5.0global_frame: maprobot_base_frame: base_linkresolution: 0.05rolling_window: falsetrack_unknown_space: trueplugins: - {name: explore_boundary, type: "frontier_exploration::BoundedExploreLayer"}- {name: sensor,           type: "costmap_2d::ObstacleLayer"}- {name: inflation,        type: "costmap_2d::InflationLayer"}explore_boundary:resize_to_boundary: truefrontier_travel_point: closestsensor:observation_sources: laserlaser: {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>

2.2. 使用 /map 數據進行探索

在使用全局 /map 信息源（來自 map_server 或 gmapping）運行動作服務器/客戶端時，勘探成本地圖的大小/分辨率將與靜態圖層加載的外部地圖源地圖相匹配，因此必須禁用 resize_too_boundary 參數，并且勘探成本地圖的 global_frame 必須與外部 /map 相匹配。

使用 gmapping 進行探索時，還必須禁用 explore_clear_space，以防止節點重新探索已知區域。

啟動文件示例：global_map.launch

<launch><!-- Set to your sensor's range --><arg name="sensor_range" default="1.0"/><node pkg="frontier_exploration" type="explore_client" name="explore_client" output="screen"/><node pkg="frontier_exploration" type="explore_server" name="explore_server" output="screen" ><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 BoundedExploreLayer<rosparam ns="explore_costmap" subst_value="true">footprint: [[0.1, 0.0], [0.0, 0.1], [0.0, -0.1], [-0.1, 0.0]]robot_radius: 0.10transform_tolerance: 0.5update_frequency: 5.0publish_frequency: 5.0#must match incoming static mapglobal_frame: maprobot_base_frame: base_linkresolution: 0.05rolling_window: falsetrack_unknown_space: trueplugins: - {name: static,           type: "costmap_2d::StaticLayer"}            - {name: explore_boundary, type: "frontier_exploration::BoundedExploreLayer"}#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: trueexplore_boundary:resize_to_boundary: falsefrontier_travel_point: middle#set to false for gmapping, true if re-exploring a known areaexplore_clear_space: falsesensor:observation_sources: laserlaser: {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>

三、提供的組件

1、explore_client

explore_client 節點偵聽 Rviz 發布的點，并構建一個 ExploreTask 行動目標發送給 explore_server。

1.1. 調用的操作

• explore_server (frontier_exploration/ExploreTask)
  向 explore_server 發送探索目標的客戶端。

1.2. 訂閱主題

• /clicked_point (geometry_msgs/PointStamped)
  從 rviz 工具點擊的點。

1.3. 發布主題

• exploration_polygon_marker （visualization_msgs/Marker）
  通過點擊點實現邊界可視化。

2、explore_server

explore_server 節點為所有已連接的客戶端執行探索操作。它使用 costmap_2d 對象來跟蹤探索進度，并在必要時為 move_base 創建移動目標。

2.1. 提供的操作

• explore_server (frontier_exploration/ExploreTask)
  接收勘探任務請求的服務器

2.2. 調用的操作

• move_base（move_base_msgs/MoveBaseAction）
  向 move_base 發送移動目標的客戶端。

2.3. 調用的服務

• ~explore_costmap/explore_boundary/update_boundary_polygon (frontier_exploration/UpdateBoundaryPolygon)
  (來自 Costmap 的內部服務）為勘探任務設置邊界。
• ~explore_costmap/explore_boundary/get_next_frontier (frontier_exploration/GetNextFrontier)
  (來自 Costmap 的內部服務）獲取下一個要探索的邊界的姿態。

2.4. 參數

• ~explore_costmap （插件）
  內部 costmap 圖層的配置，預計包含 BoundedExploreLayer。
• ~frequency （浮點數，默認值：0.0）
  為下一個邊界目標重新處理代價圖的頻率。如果頻率為 0.0，則只有在通過 move_base 達到上一個邊界目標時，才會詢問新的邊界目標。頻率越高，提交 move_base 目標的頻率越高，探索過程也就越 “平滑”。
• ~goal_aliasing （浮點數，默認值：0.1）
  當頻率 > 0.0 時，~goal_aliasing 是在新目標提交給 move_base 之前，上一個目標和新目標之間所需的距離 delta。在 sensor_range/2 > ~goal_aliasing > 0.0 范圍內的任何位置設置都是安全的，而且該參數將減少 "平滑 "探索過程中發送的冗余目標數量。

3、BoundedExploreLayer

frontier_exploration::BoundedExploreLayer 層是一個 costmap_2d 插件，它實現了執行邊界探索任務所需的若干功能。

3.1. 發布主題

• ~frontiers (sensor_msgs/PointCloud2)
  點云 pcl::Pointcloud< pcl::PointXYZI> 在調用 ~get_next_frontier 服務時標記檢測到的所有邊界，并使用強度較高的點標記所選邊界。

3.2. 服務

• ~update_boundary_polygon (frontier_exploration/UpdateBoundaryPolygon)
  為探索任務設置邊界。
• ~get_next_frontier (frontier_exploration/GetNextFrontier)
  獲取下一個探索邊界的姿態。

3.3. 參數

• ~resize_too_boundary （bool，默認：false）
  當通過 ~update_boundary_polygon 接收到多邊形邊界時，會根據邊界極值調整圖層父成本貼圖的大小。
• ~frontier_travel_point （字符串，默認：最近）
  通過 ~get_next_frontier 輸出下一個邊界的姿態時，定義要輸出為 pose.position 的邊界幾何屬性。可用：離機器人最近的點、邊界的中間點、所有邊界點的中心點（笛卡爾平均值）。
• ~explore_clear_space （bool，默認：true）
  配置探索任務是探索所有清晰空間（true），還是只探索未知空間（false）。

算法流程圖如下所示：

參考：

1. https://github.com/paulbovbel/frontier_exploration
2. http://wiki.ros.org/frontier_exploration
3. ROS學習筆記之——基于frontier_exploration的機器人自主探索（未成功運行）
4. 結合frontier_exploration + gmapping + move_base包實現指定區域建圖
5. 【SLAM】ROS平臺下三種自主探索算法總結