ipa 功能包调试，分区算法，覆盖算法测试

参考

wiki 流网络 flow network 解释

相关文章

ipa 覆盖算法测试

ipa 分区算法 ipa 分区算法总结，部分算法图解

ipa 覆盖算法分析（一）

ipa 覆盖算法分析（二）

环境

ubuntu20，ros 版本 noetic

运行测试

按照 readme 提示进行测试，跳过第一个步骤，并不需要 turtlebot3。

执行第三个 launch 报错：

看下 room_exploration_client.launch 文件

<?xml version="1.0"?>
<launch>

        <arg name="env_pack" default="$(find cob_default_env_config)" />
        <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)"/>
        <arg name="robot_radius" default="0.5"/>
        <arg name="coverage_radius" default="0.5"/>
        <arg name="use_test_maps" default="true"/>

        <!--  -->
        <node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
                <rosparam file="$(arg env_pack)/envs/$(arg robot_env)/map.yaml" command="load" />
                <param name="env_pack" value="$(arg env_pack)"/>
                <param name="robot_env" value="$(arg robot_env)"/>
                <param name="robot_radius" value="$(arg robot_radius)"/>
                <param name="coverage_radius" value="$(arg coverage_radius)"/>
                <param name="use_test_maps" value="$(arg use_test_maps)"/>
        </node>


</launch>

env_pack：ipa 的配置功能包，实践中将程序和系统配置分离，相关功能配置可能是在项目中专门配置功能包中。

robot_env：机器环境，在 env_pack 包中，对多个机器（差速轮底盘，阿克曼底盘等）有多个不同的配置，需要根据机器分别配置参数。

robot_radius：？？？作用

coverage_radius：机器的覆盖半径。

use_test_maps：是否用 ipa 官方的测试地图。

默认的配置功能包是 cob_default_env_config ，我们自己的项目中不存在这个功能包，所以要创建一个新功能包来完成 ipa 配置功能包即可。

在 ipa_coverage_planning 文件包内创建 ipa_env 功能包，用 tree 查看目录如下

➜  ipa_coverage_planning git:(develop) ✗ tree -L 1
.
├── ipa_building_msgs
├── ipa_building_navigation
├── ipa_coverage_planning
├── ipa_env
├── ipa_room_exploration
├── ipa_room_segmentation
└── README.md

ipa_env 包目录如下：

第 3 个 launch 指令执行需要修改 env_pack 参数值，如下：

roslaunch ipa_room_exploration room_exploration_client.launch env_pack:=ipa_env robot_env:=ipa_robot use_test_maps:=false

执行后依旧报错：

把 room_exploration_client.launch 的 env_pack 默认值修改为存在的其他功能包名，如下：

<arg name="env_pack" default="$(find ipa_room_exploration)" />

执行后没有“not found cob_default_env_config” 的错误了。

！！！注意！！！

这里说明 xml 中标签 <arg> 的默认值执行是在终端赋值之前的执行的！

但有了其他问题，rosparam 找不到 map.yaml 文件：

在 map.yaml 文件目录下执行 pwd 查看路径：

路径是正确的。

仔细看看 xml，发现 file 是用 $(arg env_pack) 拼接的，说明变量 env_pack 需要包含 ipa 配置功能包在系统中的绝对路径，而我们的传参仅仅传递了 ipa 配置功能包名。

修改 launch 如下，让 file 自动寻找 env_pack 功能包在系统的绝对路径，下面只显示修改的部分：

  <arg name="env_pack" default="ipa_room_exploration" />
  <node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
    <rosparam file="$(find $(arg env_pack))/envs/$(arg robot_env)/map.yaml" command="load" />
    <param name="env_pack" value="$(find env_pack)"/>
  </node>

再执行 launch，map.yaml 文件已经可以找到了，但出现新问题，xml 中 $() 不让嵌套使用，报语法错误｡ﾟ(TヮT)ﾟ｡

最后还是改成下面这样：

<?xml version="1.0"?>
<launch>

        <arg name="env_pack" default="$(find ipa_env)" />
        <arg name="robot_env" default="$(optenv ROBOT_ENV !!NO_ROBOT_ENV_SET!!)"/>
        <arg name="robot_radius" default="0.5"/>
        <arg name="coverage_radius" default="0.5"/>
        <arg name="use_test_maps" default="true"/>

        <!--  -->
        <node ns="room_exploration" pkg="ipa_room_exploration" type="room_exploration_client" name="room_exploration_client" output="screen">
                <rosparam file="$(arg env_pack)/envs/$(arg robot_env)/map.yaml" command="load" />
                <param name="env_pack" value="$(arg env_pack)"/>
                <param name="robot_env" value="$(arg robot_env)"/>
                <param name="robot_radius" value="$(arg robot_radius)"/>
                <param name="coverage_radius" value="$(arg coverage_radius)"/>
                <param name="use_test_maps" value="$(arg use_test_maps)"/>
        </node>


</launch>

执行 launch 也不需要修改 env_pack 变量值了：

roslaunch ipa_room_exploration room_exploration_client.launch robot_env:=ipa_robot use_test_maps:=false

程序运行成功！

测试地图

ipa 默认覆盖算法

覆盖算法测试

ipa_room_exploration/ros/launch/room_exploration_action_server_params.yaml 文件中的 room_exploration_algorithm 参数来选择覆盖算法。重启 ipa 服务端，重新加载参数，执行

roslaunch ipa_room_exploration room_exploration_action_server.launch

执行后发现覆盖路径无明显变化。

服务端 log 如下：

log 显示启动 server.launch 后，覆盖算法已经修改为 6 局部能量最小覆盖算法。

注意这里的 room_exploration/path_planning_algorithm 的 room_exploration 并非参数空间前缀，而是代码这么写的。一开始还被误导以为是参数空间名对不上导致的问题。

之后再启动 client.launch，触发了动态参数服务器，将覆盖算法改为了 8，牛耕算法变种。

在 ipa_room_exploration/ros/src/room_exploration_action_client.cpp 的 121 行发现这段代码，这里创建了动态参数服务器的客户端，修改了参数：

屏蔽这段代码，编译 ipa_room_exploration 功能包，重新执行服务器 launch 和客户端 launch。

成功执行了局部能量最小覆盖算法！

ipa 新覆盖算法

下面展示所有的覆盖算法路径效果，这里的测试仅修改了 room_exploration_algorithm 服务器参数，其他参数是官方默认参数。

• room_exploration_algorithm 参数值之 grid point explorator

对应论文的 Grid-based Traveling Salesman Coverage Path Planning 基于栅格旅行商覆盖路径算法，用 TSP 旅行商算法来计算地图中所有空闲栅格的遍历顺序。

该算法十分耗时，作者甚至给它做了个进度条，用一张 gif 来感受一下有多慢：

ipa TSP 遍历覆盖算法

• room_exploration_algorithm 参数值之 boustrophedon explorator

对应论文的 Boustrophedon Coverage Path Planning 牛耕覆盖路径算法

ipa 牛耕覆盖算法

• room_exploration_algorithm 参数值之 neural network explorator

神经网络覆盖算法

ipa 神经网络覆盖算法

• room_exploration_algorithm 参数值之 convexSPP explorator

Convex Sensor Placement Coverage Path Planning 凸传感器放置覆盖路径算法

Log 有点多 ...

ipa 凸包 SPP 覆盖算法

• room_exploration_algorithm 参数值之 flowNetwork explorator

论文参考文献和源码中都找不到相关论文。注释有这么一句：

This class provides a coverage path planning algorithm based on a flow network.

Wiki 对流网络 flow network 的解释如下（有些资料叫 network flow 网络流）：
在图论中，流网络是一个有向图，每条边都有一个容量，每条边接收一个流。边的流量不能超过边的容量。在运筹学 operations research 中，有向图通常称为网络，顶点称为节点，边称为弧。流必须满足流入节点的流量等于流出节点的流量的限制，除非它是只有流出的源 S，或者只有流入的汇 t。

流网络，源 S，汇 t

Log 刷屏非常多，似乎在第三方库中，源码搜索不到相关日志。程序陷入死循环一直没有结束，该算法应该未完成。

• room_exploration_algorithm 参数值之 energyFunctional explorator

对应论文 Grid-based Local Energy Minimization 基于栅格的局部能量最小算法

ipa 局部能量最小覆盖算法

• room_exploration_algorithm 参数值之 voronoi explorator

对应论文 Contour Line-based Coverage Path Planning 基于轮廓线的覆盖路径算法

执行后没有规划成功，服务端 log：

把服务器参数 revisit_areas 设置为 true，再次测试...还是规划失败。

客户端 log：

没有其他 log 提示了，初步判断是地图没有封闭空间导致的，给地图增加一圈黑边（障碍物）再测试。

规划成功！说明用 voronoi explorator 算法需要地图有封闭边界！

ipa 轮廓线覆盖算法

• room_exploration_algorithm 参数值之 boustrophedon variant explorator

牛耕法变种，改进了牛耕法中细胞分解后，对相同主轴的分区进行合并，减少区域的碎片化。没找到对应的论文信息。

ipa 牛耕法变种覆盖算法

分区算法测试

分区算法在 ipa_room_segmentation 功能包中，也是分为客户端和服务端两部分运行：

服务端 room_segmentation_action_server.launch 和客户端 room_segmentation_action_client.launch

分区算法的参数配置在 ipa_room_segmentation/ros/launch/room_segmentation_action_server_params.yaml 中。

同样，客户端代码也修改了动态参数服务器的参数，先屏蔽了 ipa_room_segmentation/ros/src/room_segmentation_client.cpp 的部分代码：

编译后分别运行服务端和客户端的 launch。

测试的地图不对，测试的还是官方的测试地图：

ipa 分区测试默认地图

而我的地图是：

ipa 分区测试用户地图

从 log 看参数配置的生效了。再看看分区客户端代码是怎么写的...

客户端是直接把测试地图名写入一个 vector 变量 map_names 中，然后在 for 循环中在指定地址获取相应的地图地址，把地图转为地图话题数据，再装入 goal 中发送给服务端。

流程大概如下：

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

  // map names
  std::vector< std::string > map_names;
  map_names.push_back("lab_ipa"); // 这就是测试的第一张默认地图
  map_names.push_back("lab_c_scan");
  ... // 加了很多地图

  for (size_t image_index = 0; image_index<map_names.size(); ++image_index) {
    // 在 ipa_room_segmentation 功能包的指定位置搜索地图
    std::string image_filename = ros::package::getPath("ipa_room_segmentation") + "/common/files/test_maps/" + map_names[image_index] + ".png";
    cv::Mat map = cv::imread(image_filename.c_str(), 0); // cv 读地图
    ...
    cv_image.toImageMsg(labeling); // 数据转换
    actionlib::SimpleActionClient<ipa_building_msgs::MapSegmentationAction> ac("room_segmentation_server", true);
    ... // goal 数据装包
    ac.sendGoal(goal); // 目标发送到服务端
    bool finished_before_timeout = ac.waitForResult(ros::Duration()); // 等待结果

    if (finished_before_timeout) {
      ... // 数据转换到 cv 格式
      cv::imshow("segmentation", colour_segmented_map); // cv 显示地图
      cv::waitKey(); // 敲空格测试下一张地图
    }
  } // for
  return 0;
}

所以只需要修改客户端的测试地图名称和测试地图所在的路径即可。

根据自己的测试环境修改这两部分代码，编译后测试 ok。

下面展示所有的分割算法分区效果，这里的测试仅修改了 room_segmentation_algorithm 服务器参数，其他参数是官方默认参数。

• room_segmentation_algorithm 参数值之 morphological segmentation

对应论文的 Morphological Segmentation 形态分割算法

ipa 形态分割算法

• room_segmentation_algorithm 参数值之 distance segmentation

对应论文 Distance Transform-based Segmentation 距离变换分割算法

ipa 距离变换分割算法

• room_segmentation_algorithm 参数值之 Voronoi segmentation

对应论文 Voronoi Graph-based Segmentation Voronoi 图分割算法

ipa Voronoi 图分割算法

• room_segmentation_algorithm 参数值之 semantic segmentation

在《Room Segmentation: Survey, Implementation, and Analysis.》称为Feature-based Segmentation 特征/语义分割

ipa 语义分割算法

• room_segmentation_algorithm 参数值之 voronoi random field segmentation

对应论文 Voronoi Random Fields Segmentation Voronoi 随机势场分割

ipa Voronoi 随机势场分割算法

• room_segmentation_algorithm 参数值之 passthrough segmentation

不分割算法，正如其名...

ipa 不分割算法