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

参考

wiki 流网络 flow network 解释

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_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。

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

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

room_exploration_algorithm 参数值之 grid point explorator

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

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

room_exploration_algorithm 参数值之 boustrophedon explorator

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

room_exploration_algorithm 参数值之 neural network explorator

神经网络覆盖算法

room_exploration_algorithm 参数值之 convexSPP explorator

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

Log 有点多 ...

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。

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

room_exploration_algorithm 参数值之 energyFunctional explorator

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

room_exploration_algorithm 参数值之 voronoi explorator

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

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

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

客户端 log：

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

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

room_exploration_algorithm 参数值之 boustrophedon variant explorator

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

分区算法测试

分区算法在 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。

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

而我的地图是：

从 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 服务器参数，其他参数是官方默认参数。