逻辑参考：ROS全覆盖规划算法逻辑整理笔记
ipa_room_exploration/room_exploration_action_server.cpp
void RoomExplorationServer::exploreRoom(const ipa_building_msgs::RoomExplorationGoalConstPtr &goal)
定义了地图的原点和分辨率
const cv::Point2d map_origin(goal->map_origin.position.x, goal->map_origin.position.y); const float map_resolution = goal->map_resolution; // in [m/cell] const float map_resolution_inverse = 1./map_resolution;
定义了机器人的半径，并将其转换为像素单位
const float robot_radius = goal->robot_radius; const int robot_radius_in_pixel = (robot_radius / map_resolution);
定义了机器人的起始位置，并将其转换为像素单位
const cv::Point starting_position((goal->starting_position.x-map_origin.x)/map_resolution, (goal->starting_position.y-map_origin.y)/map_resolution);
设置规划模式
planning_mode_ = goal->planning_mode; if (planning_mode_==PLAN_FOR_FOOTPRINT) std::cout << "planning mode: planning coverage path with robot's footprint" <<std::endl; else if (planning_mode_==PLAN_FOR_FOV) std::cout << "planning mode: planning coverage path with robot's field of view" <<std::endl;
将ROS中的地图数据转换为OpenCV可以处理的格式
cv_bridge::CvImagePtr cv_ptr_obj; cv_ptr_obj = cv_bridge::toCvCopy(goal->input_map, sensor_msgs::image_encodings::MONO8); cv::Mat room_map = cv_ptr_obj->image;
计算房间的面积，通过统计图像中符合特定条件（像素值大于或等于250）的像素数量，然后将这个像素数量转换为实际的面积单位（例如平方米）
int area_px = 0; // room area in pixels for (int v=0; v<room_map.rows; ++v) for (int u=0; u<room_map.cols; ++u) if (room_map.at<uchar>(v,u) >= 250) area_px++; std::cout << "### room area = " << area_px*map_resolution*map_resolution << " m^2" << std::endl;
通过腐蚀和膨胀操作对地图进行预处理，以改善地图的质量，忽略不可达区域和地图错误/伪影，从而提高后续处理的准确性和效率
【补充理解腐蚀膨胀操作：OpenCV 理解腐蚀与膨胀(开、闭运算以及形态学方法)】
cv::Mat temp; //声明了一个名为temp的cv::Mat对象，用于临时存储图像处理的中间结果 cv::erode(room_map, temp, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_); //使用OpenCV的erode函数对room_map图像进行腐蚀操作。腐蚀操作可以去除图像中的小白点（噪声），有助于忽略不可达区域和地图错误/伪影。temp是腐蚀操作的结果，存储在temp对象中。cv::Mat()表示使用默认的结构元素，cv::Point(-1, -1)表示使用默认的腐蚀半径，map_correction_closing_neighborhood_size_是腐蚀操作的半径大小。 cv::dilate(temp, room_map, cv::Mat(), cv::Point(-1, -1), map_correction_closing_neighborhood_size_); //使用OpenCV的dilate函数对temp图像进行膨胀操作。膨胀操作可以填补腐蚀后可能出现的小黑洞，有助于恢复地图的完整性。room_map是膨胀操作的结果，存储在原始的room_map对象中。cv::Mat()表示使用默认的结构元素，cv::Point(-1, -1)表示使用默认的膨胀半径，map_correction_closing_neighborhood_size_是膨胀操作的半径大小。
保留最大连通域，事实上在这里我有个疑惑，地图本身就是机器人能去的地方建成的，为什么会存在不连通的地方？
const bool room_not_empty = removeUnconnectedRoomParts(room_map);
这里插入removeUnconnectedRoomParts函数的实现方式，具体通过5-6步操作，
1. 创建新的整数类型地图：函数首先创建一个新的cv::Mat对象room_map_int，其数据类型为CV_32SC1（即32位单通道整数）。这个新地图用于存储每个像素的标签信息。
2. 标记不可达区域：函数遍历原始地图room_map的每个像素。如果像素值为255（通常表示不可达区域），则在room_map_int中将该像素的值设置为-100。否则，将该像素的值设置为0。
3. 使用洪水填充算法标记连通区域：函数再次遍历room_map_int，对于每个值为-100的像素（即不可达区域），使用cv::floodFill函数进行洪水填充。这个函数会填充连续的像素区域，并返回该区域的面积。每个独立区域的面积和对应的标签（label）被存储在一个映射（map）中。
4. 确定最大连通区域：函数检查映射中的元素，找到面积最大的连通区域的标签。
5. 移除非最大连通区域的像素：最后，函数遍历room_map_int，将所有不属于最大连通区域的像素在原始地图room_map中设置为0（即不可达）。
6. 返回结果：如果成功移除了所有不连通的部分，函数返回true；如果没有找到任何连通区域，函数返回false。
bool RoomExplorationServer::removeUnconnectedRoomParts(cv::Mat& room_map)
{
// create new map with segments labeled by increasing labels from 1,2,3,...
cv::Mat room_map_int(room_map.rows, room_map.cols, CV_32SC1);
for (int v=0; v<room_map.rows; ++v)
{
for (int u=0; u<room_map.cols; ++u)
{
if (room_map.at<uchar>(v,u) == 255)
room_map_int.at<int32_t>(v,u) = -100;
else
room_map_int.at<int32_t>(v,u) = 0;
}
}
std::map<int, int> area_to_label_map; // maps area=number of segment pixels (keys) to the respective label (value)
int label = 1;
for (int v=0; v<room_map_int.rows; ++v)
{
for (int u=0; u<room_map_int.cols; ++u)
{
if (room_map_int.at<int32_t>(v,u) == -100)
{
const int area = cv::floodFill(room_map_int, cv::Point(u,v), cv::Scalar(label), 0, 0, 0, 8 | cv::FLOODFILL_FIXED_RANGE);
area_to_label_map[area] = label;
++label;
}
}
}
// abort if area_to_label_map.size() is empty
if (area_to_label_map.size() == 0)
return false;
// remove all room pixels from room_map which are not accessible
const int label_of_biggest_room = area_to_label_map.rbegin()->second;
std::cout << "label_of_biggest_room=" << label_of_biggest_room << std::endl;
for (int v=0; v<room_map.rows; ++v)
for (int u=0; u<room_map.cols; ++u)
if (room_map_int.at<int32_t>(v,u) != label_of_biggest_room)
room_map.at<uchar>(v,u) = 0;
return true;
}
为机器人探索任务确定合适的网格大小
1. 初始化变量：定义了几个变量来存储网格大小、视野（Field of View, FOV）的相关参数和圆的半径。
2. 根据规划模式计算网格大小：
如果规划模式是基于视野（FOV）的（PLAN_FOR_FOV），则从给定的FOV角度计算出一个圆，使得整个网格可以被这个圆覆盖。这涉及到读取FOV角度，计算圆心和半径，然后根据这个圆的半径计算网格单元的大小。
如果规划模式是基于机器人的覆盖半径，那么直接根据覆盖半径计算网格单元的大小。
3. 将网格大小从米转换为像素：使用地图分辨率将网格大小从米转换为像素单位。
4. 输出网格大小：打印出网格的大小，包括米和像素单位。
5. 设置网格单元大小：如果cell_size_（网格单元大小）未提供或小于等于0，则将其设置为计算出的网格大小。
// get the grid size, to check the areas that should be revisited later
double grid_spacing_in_meter = 0.0; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius
float fitting_circle_radius_in_meter = 0;
Eigen::Matrix<float, 2, 1> fitting_circle_center_point_in_meter; // this is also considered the center of the field of view, because around this point the maximum radius incircle can be found that is still inside the fov
std::vector<Eigen::Matrix<float, 2, 1> > fov_corners_meter(4);
const double fov_resolution = 1000; // in [cell/meter]
if(planning_mode_ == PLAN_FOR_FOV) // read out the given fov-vectors, if needed
{
// Get the size of one grid cell s.t. the grid can be completely covered by the field of view (fov) from all rotations around it.
for(int i = 0; i < 4; ++i)
fov_corners_meter[i] << goal->field_of_view[i].x, goal->field_of_view[i].y;
computeFOVCenterAndRadius(fov_corners_meter, fitting_circle_radius_in_meter, fitting_circle_center_point_in_meter, fov_resolution);
// get the edge length of the grid square that fits into the fitting_circle_radius
grid_spacing_in_meter = fitting_circle_radius_in_meter*std::sqrt(2);
}
else // if planning should be done for the footprint, read out the given coverage radius
{
grid_spacing_in_meter = goal->coverage_radius*std::sqrt(2);
}
// map the grid size to an int in pixel coordinates, using floor method
const double grid_spacing_in_pixel = grid_spacing_in_meter/map_resolution; // is the square grid cell side length that fits into the circle with the robot's coverage radius or fov coverage radius, multiply with sqrt(2) to receive the whole working width
std::cout << "grid size: " << grid_spacing_in_meter << " m (" << grid_spacing_in_pixel << " px)" << std::endl;
// set the cell_size_ for #4 convexSPP explorator or #5 flowNetwork explorator if it is not provided
if (cell_size_ <= 0)
cell_size_ = std::floor(grid_spacing_in_pixel);
后续部分为调用具体的规划算法进行规划；
具体参数：
int room_exploration_algorithm_; // variable to specify which algorithm is going to be used to plan a path // 1: grid point explorator // 2: boustrophedon explorator // 3: neural network explorator // 4: convexSPP explorator // 5: flowNetwork explorator // 6: energyFunctional explorator // 7: Voronoi explorator // 8: boustrophedon variant explorator
由于常用牛耕法，这里就写room_exploration_algorithm_ == 2的情况
if (room_exploration_algorithm_ == 2) // use boustrophedon explorator
{
// plan path
if(planning_mode_ == PLAN_FOR_FOV)
boustrophedon_explorer_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, false, fitting_circle_center_point_in_meter, min_cell_area_, max_deviation_from_track_);
else
boustrophedon_explorer_.getExplorationPath(room_map, exploration_path, map_resolution, starting_position, map_origin, grid_spacing_in_pixel, grid_obstacle_offset_, path_eps_, cell_visiting_order_, true, zero_vector, min_cell_area_, max_deviation_from_track_);
}
后续调用的规划算法可在ipa_room_exploratopm/common/src/boustrophedon_explorator.cpp中查看
getExplorationPath函数接口定义如下：
void BoustrophedonExplorer::getExplorationPath(const cv::Mat& room_map, std::vector<geometry_msgs::Pose2D>& path, const float map_resolution, const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel, const double grid_obstacle_offset, const double path_eps, const int cell_visiting_order, const bool plan_for_footprint, const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track)
getExplorationPath函数的参数说明：
const cv::Mat& room_map: 输入的房间地图，通常是一个二维矩阵，表示房间的布局。
std::vector
const float map_resolution: 地图的分辨率，即每个像素或单元格代表的实际面积大小。
const cv::Point starting_position: 探索的起始位置。
const cv::Point2d map_origin: 地图的原点坐标。
const double grid_spacing_in_pixel: 网格间距（以像素为单位）。
const double grid_obstacle_offset: 网格障碍物偏移量。
const double path_eps: 路径精度参数。
const int cell_visiting_order: 单元格访问顺序，可以是最优的旅行商问题（TSP）解决方案或简单的从左到右的顺序。
const bool plan_for_footprint: 是否为机器人的实际占用空间（footprint）规划路径。
const Eigen::Matrix
const double min_cell_area: 最小单元格面积。
const int max_deviation_from_track: 最大偏离轨迹的程度。
getExplorationPath函数的操作流程包括：
1.计算房间地图的主要方向，并根据这个方向和额外的旋转偏移量来确定旋转角度。
2.确定旋转中心，如果没有提供，则根据房间地图的最小和最大坐标点来确定。
3.计算旋转矩阵，使用OpenCV的 cv::getRotationMatrix2D 函数。
4.确定旋转后的边界矩形，以便知道新图像的大小。
5.调整变换矩阵，以确保旋转后的图像正确对齐。
6.返回旋转角度。
