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 不分割算法

  • 30
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
ipa_coverage_planning 是一种全覆盖算法,用于规划无人机的航线,以实现对一个区域的全面覆盖。下面对其代码进行详解。 首先,算法需要一个网格来表示待覆盖的区域。算法中定义了一个 Grid 类来表示网格,并在其构造函数中初始化了网格的大小和分辨率。网格中的每个单元格都用一个数字表示其状态,0 表示未覆盖的区域,1 表示已覆盖的区域。 算法的入口函数是 coverage_planning 函数。在这个函数中,首先创建一个无人机对象,通过传递网格和无人机的相关参数进行初始化。然后进入循环,直到整个区域被完全覆盖为止。 循环中的每一步都遵循以下步骤: 1. 首先,从当前无人机位置开始,计算覆盖区域中离无人机最近的点,作为下一个目标点。 2. 然后,使用某种路径规划算法(例如 A* 算法计算从当前位置到目标点的最短路径。 3. 接下来,通过无人机以固定速度沿着路径移动到目标点,并在移动过程中更新每个单元格的覆盖状态。 4. 当无人机到达目标点后,它将停留一段时间,以达到完全覆盖该点的效果。 5. 最后,重新计算下一个目标点,并重复以上步骤。 在算法的实现中,还需要考虑以下几个方面: 1. 无人机的速度和停留时间的设定,以确保完全覆盖每个点的要求。 2. 路径规划算法的选择和实现,以确保无人机能够找到最短路径。 3. 网格的边界条件处理,防止无人机越界。 通过以上详细的代码解释,我们可以更好地理解并实现 ipa_coverage_planning 算法,以实现全覆盖航线的规划。
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值