ROS Turtlebot3 多机协同自主探索环境时遇到的多机任务分配问题，explore节点

Question

大家好，我现在在做ROS机器人相关的本科毕设，主要方向是多车自主协同建图，用到的功能包是explore_lite。
现在实现了多车探索建图，简单来说就是运行了三套独立的自主探索系统，每辆车作为一个独立的个体去探索环境，但是这个过程中小车没法相互配合，有时候它们都会认为某个区域是空白，然后都跑去探索同一个区域，这样多个小车的探索路径会重复，导致探索效率低。
我初步认为是每个小车的explore节点只基于本车建立的/tb3_x/map并进行未知区域寻找，为本车提供相应的路径规划，所以有时候小车都会去一个相同的地方。
那么如何写一个节点，使得它能订阅三个小车融合后的地图/map，对其进行未知区域寻找，并且将找到的多个未知区域分别发送给不同小车做路径规划，去到目标点进行探索呢？
或者修改explore源码，我看了对explore_lite源码的解读，关键在于找到边界frontiers后如何选取三个目标点，并且发布相应的话题/tb3_x/goal（类似）到小车move_base模块进行路径规划呢。

Answer 1

引自免费微信小程序：皆我百晓生

在解决您所描述的多车协同自主探索环境时的任务分配问题，我们需要设计一个中心节点来接收并融合所有小车的地图信息，识别未探索区域，并根据某种策略将这些区域合理地分配给各个小车。以下是实现这一功能的一种详细方案：

1. 创建中心节点

首先，创建一个名为multi_robot_explore_manager的新节点，负责订阅所有小车的融合地图、识别未探索区域，并向各小车分配探索任务。在C++中，可以使用以下结构来初始化节点：

#include <ros/ros.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "multi_robot_explore_manager");
  ros::NodeHandle nh;

  // ... 进行后续的订阅、发布与处理逻辑

  ros::spin();
  return 0;
}

2. 订阅多车融合地图

假设您已经有一个节点负责融合三台Turtlebot3的地图信息并发布到/global_map话题。在multi_robot_explore_manager节点中订阅此话题：

#include <nav_msgs/OccupancyGrid.h>

ros::Subscriber map_subscriber = nh.subscribe<nav_msgs::OccupancyGrid>("/global_map", 1, &mapCallback);

定义对应的回调函数mapCallback来处理接收到的融合地图数据：

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& global_map_msg) {
  // 在此函数内识别未探索区域并分配任务
}

3. 识别未探索区域（Frontiers）

您可以使用frontier_exploration库（https://github.com/paulbovbel/frontier_exploration）中的frontierExplorer类来识别未探索区域。在mapCallback函数中创建一个frontierExplorer实例并调用其findFrontiers()方法：

#include <frontier_exploration/explore_client.h>
#include <frontier_exploration/Frontier.h>

frontier_exploration::frontierExplorer explorer;

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& global_map_msg) {
  explorer.setMap(*global_map_msg);
  std::vector<frontier_exploration::Frontier> frontiers;
  explorer.findFrontiers(frontiers);

  // ... 分配任务给各小车
}

4. 任务分配策略

根据您的需求，可以选择合适的任务分配策略，例如轮询、优先级排序等。这里以简单的轮询策略为例：

std::vector<std::string> robot_names = {"tb3_1", "tb3_2", "tb3_3"};
size_t current_robot_index = 0;

void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& global_map_msg) {
  // ...

  for (const auto& frontier : frontiers) {
    if (current_robot_index >= robot_names.size()) {
      current_robot_index = 0;  // 循环回到第一个小车
    }

    const std::string& current_robot_name = robot_names[current_robot_index++];
    sendExplorationGoal(current_robot_name, frontier);
  }
}

void sendExplorationGoal(const std::string& robot_name, const frontier_exploration::Frontier& frontier) {
  // ... 构造并发布目标点到相应小车的/goal话题
}

5. 发布探索目标到各小车

在sendExplorationGoal函数中，根据给定的小车名称和未探索区域（Frontier），构造一个geometry_msgs::PoseStamped目标点消息，并发布到对应小车的/tb3_x/move_base_simple/goal话题（其中x为小车编号）：

#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseActionGoal.h>

void sendExplorationGoal(const std::string& robot_name, const frontier_exploration::Frontier& frontier) {
  geometry_msgs::PoseStamped goal_pose;
  goal_pose.header.frame_id = "map";
  goal_pose.pose.position.x = frontier.centroid.x;
  goal_pose.pose.position.y = frontier.centroid.y;
  goal_pose.pose.orientation.w = 1.0;  // 目标点朝向默认向前

  move_base_msgs::MoveBaseActionGoal goal;
  goal.goal.target_pose = goal_pose;

  ros::Publisher goal_publisher = nh.advertise<move_base_msgs::MoveBaseActionGoal>(
      ("/" + robot_name + "/move_base_simple/goal"), 1);
  goal_publisher.publish(goal);
}

综上所述，通过创建multi_robot_explore_manager节点并实现上述逻辑，您就可以实现多车协同自主探索环境的任务分配。请注意，实际应用中可能需要根据项目需求调整任务分配策略、融合地图的方式以及相关参数设置。同时，确保所有节点间的时间同步和通信正常，以保证多车协作的高效性和稳定性。