0.本文初心：

一直有朋友在我博文中關于掃地機器人（側重區域覆寫演算法）和物流機器人（側重運輸調度演算法）留言交流，受限于時間一直沒有完整回復，這段時間稍稍有空，加班由7127變成了997，終于可以寫一篇了，

1.預備知識：

如何讓忙碌了一天的程式員到家后發現地面一塵不染，做一個掃地機器人吧，很難嗎？當然不難，超簡單的，不信？

服務型移動機器人如何實作室內路徑全覆寫清掃給你一個清爽干凈的家（除錯完整版記錄）

1.1機器人模型

掃地機器人主要有兩種模型哦，一種兩個輪子適合普通家用，還有一種四個輪子適合體育館超市等大型空間使用，

這里以運動學模型介紹為主，動力學有點難……

請務必注意！！！這種機器人不能橫著走，不能橫著走，不能橫著走！！！

我們都知道兩輪的掃地機器人可以前進，后退，左轉和右轉，但是不能側向平移，為啥，如何更專業的描述這一特性，其數學模型給出了非常明確的答案！無論左輪和右輪如何旋轉， $\nu _{y}$ 都是等于0的啊！

機器人建立準確的數學模型，對于理解，分析和控制此系統有著非常重要的作用，

這類機器人的運動學模型有兩類，分別如下所示：

這是依據左右輪速度建立的模型，我們通常習慣用線速度和角速度去分析機器人：

設計時候使用II型，控制機器人用I型，很簡單吧，但是這還不夠啊，還需要知道我們的環境地圖，看下節->

1.2環境地圖

到了陌生的地方自然離不開地圖，回到熟悉的地方，我們有記憶中的地圖，可見地圖對于定位，導航，路徑，任務規劃等必不可少，非常重要，常見的平面地圖有如下：

大環境：

小環境：

那么我們掃地機器人需要啥地圖呢？？？如何建立這些地圖呢？？？分別由仿真和實物兩種方式，這里介紹仿真，實物放在文末，

一個典型室內環境如“初心”中的案例所示，這里再舉一個圖書館的案例：

對這個環境用機器人SLAM建立的環境地圖如下，詳細內容參考對應詳細介紹的博文：

有了這個地圖，就可以實作機器人在環境下的各種路徑規劃類相關任務設計啦，

啥是路徑規劃？？？

這張圖是怎么來的？？？

1.3路徑規劃演算法

覆寫

簡單說，覆寫就是導航點覆寫了地圖區域范圍內所有機器人可以運動到的點，并且機器人必須全部走一遍！

多么痛苦的到此一游啊！

具體參考2016年博文：掃地機器人演算法的一些想法和測驗

2.技術決議：

掃地機器人核心是區域覆寫演算法要和機器人模型相匹配，例如：

設定了不同機器人自身半徑和清掃半徑的區域覆寫圖如下：

這時候發現為了防止出現卡住等極端情況，門狹小的房間并未在清掃規劃范圍呢，

清掃非常徹底，但是路徑規劃很密集，

對于環境簡單或復雜的地圖，如果同一個演算法都能適用，那說明演算法的適應性非常好！！！

簡單地圖：

所有演算法測驗都需要經過從簡單到復雜的程序，不要急于求成啊，

復雜地圖：

這里選用“初心”中的環境構建出的地圖：

具體的清掃效果如何呢？看看，為了區別色彩調了一下：

啥，掃地速度太慢了？？？這樣機器人作業只能724，沒關系，機器人可以24小時作業的，只要“電”管夠！

如果想快一點？參考這篇博文：https://zhangrelay.blog.csdn.net/article/details/76850690

三個機器人一起清掃如何？

如果還不滿意，要用真實機器人自己除錯掃地機器人路徑規劃演算法？看看這一篇，也許能幫到你：

tianbot_mini機器人上手ROS/SLAM/Navigation究竟有多簡單？？？

3.參考文獻

Full Coverage Path Planner (FCPP)

4.核心代碼

full_coverage_path_planner.cpp

//
// Copyright [2020] Nobleo Technology"  [legal/copyright]
//
#include <list>
#include <vector>

#include "full_coverage_path_planner/full_coverage_path_planner.h"

/*  *** Note the coordinate system ***
 *  grid[][] is a 2D-vector:
 *  where ix is column-index and x-coordinate in map,
 *  iy is row-index and y-coordinate in map.
 *
 *            Cols  [ix]
 *        _______________________
 *       |__|__|__|__|__|__|__|__|
 *       |__|__|__|__|__|__|__|__|
 * Rows  |__|__|__|__|__|__|__|__|
 * [iy]  |__|__|__|__|__|__|__|__|
 *       |__|__|__|__|__|__|__|__|
 *y-axis |__|__|__|__|__|__|__|__|
 *   ^   |__|__|__|__|__|__|__|__|
 *   ^   |__|__|__|__|__|__|__|__|
 *   |   |__|__|__|__|__|__|__|__|
 *   |   |__|__|__|__|__|__|__|__|
 *
 *   O   --->> x-axis
 */

// #define DEBUG_PLOT

// Default Constructor
namespace full_coverage_path_planner
{
FullCoveragePathPlanner::FullCoveragePathPlanner() : initialized_(false)
{
}

void FullCoveragePathPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path)
{
  if (!initialized_)
  {
    ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
    return;
  }

  // create a message for the plan
  nav_msgs::Path gui_path;
  gui_path.poses.resize(path.size());

  if (!path.empty())
  {
    gui_path.header.frame_id = path[0].header.frame_id;
    gui_path.header.stamp = path[0].header.stamp;
  }

  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
  for (unsigned int i = 0; i < path.size(); i++)
  {
    gui_path.poses[i] = path[i];
  }

  plan_pub_.publish(gui_path);
}

void FullCoveragePathPlanner::parsePointlist2Plan(const geometry_msgs::PoseStamped& start,
    std::list<Point_t> const& goalpoints,
    std::vector<geometry_msgs::PoseStamped>& plan)
{
  geometry_msgs::PoseStamped new_goal;
  std::list<Point_t>::const_iterator it, it_next, it_prev;
  int dx_now, dy_now, dx_next, dy_next, move_dir_now = 0, move_dir_prev = 0, move_dir_next = 0;
  bool do_publish = false;
  float orientation = eDirNone;
  ROS_INFO("Received goalpoints with length: %lu", goalpoints.size());
  if (goalpoints.size() > 1)
  {
    for (it = goalpoints.begin(); it != goalpoints.end(); ++it)
    {
      it_next = it;
      it_next++;
      it_prev = it;
      it_prev--;

      // Check for the direction of movement
      if (it == goalpoints.begin())
      {
        dx_now = it_next->x - it->x;
        dy_now = it_next->y - it->y;
      }
      else
      {
        dx_now = it->x - it_prev->x;
        dy_now = it->y - it_prev->y;
        dx_next = it_next->x - it->x;
        dy_next = it_next->y - it->y;
      }

      // Calculate direction enum: dx + dy*2 will give a unique number for each of the four possible directions because
      // of their signs:
      //  1 +  0*2 =  1
      //  0 +  1*2 =  2
      // -1 +  0*2 = -1
      //  0 + -1*2 = -2
      move_dir_now = dx_now + dy_now * 2;
      move_dir_next = dx_next + dy_next * 2;

      // Check if this points needs to be published (i.e. a change of direction or first or last point in list)
      do_publish = move_dir_next != move_dir_now || it == goalpoints.begin() ||
                   (it != goalpoints.end() && it == --goalpoints.end());
      move_dir_prev = move_dir_now;

      // Add to vector if required
      if (do_publish)
      {
        new_goal.header.frame_id = "map";
        new_goal.pose.position.x = (it->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
        new_goal.pose.position.y = (it->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
        // Calculate desired orientation to be in line with movement direction
        switch (move_dir_now)
        {
        case eDirNone:
          // Keep orientation
          break;
        case eDirRight:
          orientation = 0;
          break;
        case eDirUp:
          orientation = M_PI / 2;
          break;
        case eDirLeft:
          orientation = M_PI;
          break;
        case eDirDown:
          orientation = M_PI * 1.5;
          break;
        }
        new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(orientation);
        if (it != goalpoints.begin())
        {
          previous_goal_.pose.orientation = new_goal.pose.orientation;
          // republish previous goal but with new orientation to indicate change of direction
          // useful when the plan is strictly followed with base_link
          plan.push_back(previous_goal_);
        }
        ROS_DEBUG("Voila new point: x=%f, y=%f, o=%f,%f,%f,%f", new_goal.pose.position.x, new_goal.pose.position.y,
                  new_goal.pose.orientation.x, new_goal.pose.orientation.y, new_goal.pose.orientation.z,
                  new_goal.pose.orientation.w);
        plan.push_back(new_goal);
        previous_goal_ = new_goal;
      }
    }
  }
  else
  {
    new_goal.header.frame_id = "map";
    new_goal.pose.position.x = (goalpoints.begin()->x) * tile_size_ + grid_origin_.x + tile_size_ * 0.5;
    new_goal.pose.position.y = (goalpoints.begin()->y) * tile_size_ + grid_origin_.y + tile_size_ * 0.5;
    new_goal.pose.orientation = tf::createQuaternionMsgFromYaw(0);
    plan.push_back(new_goal);
  }
  /* Add poses from current position to start of plan */

  // Compute angle between current pose and first plan point
  double dy = plan.begin()->pose.position.y - start.pose.position.y;
  double dx = plan.begin()->pose.position.x - start.pose.position.x;
  // Arbitrary choice of 100.0*FLT_EPSILON to determine minimum angle precision of 1%
  if (!(fabs(dy) < 100.0 * FLT_EPSILON && fabs(dx) < 100.0 * FLT_EPSILON))
  {
    // Add extra translation waypoint
    double yaw = std::atan2(dy, dx);
    geometry_msgs::Quaternion quat_temp = tf::createQuaternionMsgFromYaw(yaw);
    geometry_msgs::PoseStamped extra_pose;
    extra_pose = *plan.begin();
    extra_pose.pose.orientation = quat_temp;
    plan.insert(plan.begin(), extra_pose);
    extra_pose = start;
    extra_pose.pose.orientation = quat_temp;
    plan.insert(plan.begin(), extra_pose);
  }

  // Insert current pose
  plan.insert(plan.begin(), start);

  ROS_INFO("Plan ready containing %lu goals!", plan.size());
}

bool FullCoveragePathPlanner::parseGrid(nav_msgs::OccupancyGrid const& cpp_grid_,
                                        std::vector<std::vector<bool> >& grid,
                                        float robotRadius,
                                        float toolRadius,
                                        geometry_msgs::PoseStamped const& realStart,
                                        Point_t& scaledStart)
{
  int ix, iy, nodeRow, nodeColl;
  uint32_t nodeSize = dmax(floor(toolRadius / cpp_grid_.info.resolution), 1);  // Size of node in pixels/units
  uint32_t robotNodeSize = dmax(floor(robotRadius / cpp_grid_.info.resolution), 1);  // RobotRadius in pixels/units
  uint32_t nRows = cpp_grid_.info.height, nCols = cpp_grid_.info.width;
  ROS_INFO("nRows: %u nCols: %u nodeSize: %d", nRows, nCols, nodeSize);

  if (nRows == 0 || nCols == 0)
  {
    return false;
  }

  // Save map origin and scaling
  tile_size_ = nodeSize * cpp_grid_.info.resolution;  // Size of a tile in meters
  grid_origin_.x = cpp_grid_.info.origin.position.x;  // x-origin in meters
  grid_origin_.y = cpp_grid_.info.origin.position.y;  // y-origin in meters

  // Scale starting point
  scaledStart.x = static_cast<unsigned int>(clamp((realStart.pose.position.x - grid_origin_.x) / tile_size_, 0.0,
                             floor(cpp_grid_.info.width / tile_size_)));
  scaledStart.y = static_cast<unsigned int>(clamp((realStart.pose.position.y - grid_origin_.y) / tile_size_, 0.0,
                             floor(cpp_grid_.info.height / tile_size_)));

  // Scale grid
  for (iy = 0; iy < nRows; iy = iy + nodeSize)
  {
    std::vector<bool> gridRow;
    for (ix = 0; ix < nCols; ix = ix + nodeSize)
    {
      bool nodeOccupied = false;
      for (nodeRow = 0; (nodeRow < robotNodeSize) && ((iy + nodeRow) < nRows) && (nodeOccupied == false); ++nodeRow)
      {
        for (nodeColl = 0; (nodeColl < robotNodeSize) && ((ix + nodeColl) < nCols); ++nodeColl)
        {
          int index_grid = dmax((iy + nodeRow - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0))
                            * nCols + (ix + nodeColl - ceil(static_cast<float>(robotNodeSize - nodeSize) / 2.0)), 0);
          if (cpp_grid_.data[index_grid] > 65)
          {
            nodeOccupied = true;
            break;
          }
        }
      }
      gridRow.push_back(nodeOccupied);
    }
    grid.push_back(gridRow);
  }
  return true;
}
}  // namespace full_coverage_path_planner