ROS开发专栏---基于开源导航插件 wp_map_tools 多航点巡航导航实验--适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ： 《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ： 《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》 《Ai智能体专栏） 《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

​

目录

前言

[一、wp_map_tools 插件功能介绍](#一、wp_map_tools 插件功能介绍)

[1.1 核心作用](#1.1 核心作用)

[1.2 导航通信逻辑](#1.2 导航通信逻辑)

[二、安装 wp_map_tools 导航插件](#二、安装 wp_map_tools 导航插件)

2.1、下载源码

[2.2 安装依赖](#2.2 安装依赖)

2.3、编译插件

[三、RViz 可视化添加并保存航点](#三、RViz 可视化添加并保存航点)

3.1、启动航点编辑界面

3.2、保存航点文件

四、启动导航服务

五、构建航点导航程序

5.1、编写节点代码

[5.2、修改 CMakeLists.txt 编译配置](#5.2、修改 CMakeLists.txt 编译配置)

[5.3、修改 package.xml 依赖](#5.3、修改 package.xml 依赖)

六、编译与运行

总结

---

前言

上一期博客我们已经实现了 NAV2单点自主导航，但在实际的机器人项目中，经常需要巡逻巡检、多点依次到达、自动巡航，所以逐个手动设置目标点非常麻烦。

本期博客我们来介绍一款wp_map_tools 开源导航可视化插件，此插件专门解决多航点导航问题，可以在 RViz 可视化添加、保存多个航点，支持通过话题发送指令，让机器人自动依次前往指定航点，极大简化 NAV2 二次开发难度。

适配工作空间 ：ros2_zice前置条件：已经完成 SLAM 建图、地图已放入 wpr_simulation2/maps 目录、NAV2 单点导航可以正常运行。

---

一、wp_map_tools 插件功能介绍

1.1 核心作用

1、可在RViz可视工具中添加航点，一键设置位置与朝向；

2、支持保存航点到 waypoints.yaml 文件，永久复用；

内置两个核心节点：

wp_edit_node：加载本地航点文件，提供航点查询服务；

wp_navi_server：对接 NAV2 原生导航接口，接收航点名称，自动完成导航；

提供专用话题 /waterplus/navi_waypoint、/waterplus/navi_result，开发者只需发消息即可控制机器人导航。

1.2 导航通信逻辑

发布话题 /waterplus/navi_waypoint：发送航点编号（如 1、2、3）；

订阅话题 /waterplus/navi_result：接收导航完成回调 navi done；

插件内部自动调用 NAV2 规划路径、避障行驶、到达反馈，无需手动处理底层导航逻辑。

二、安装 wp_map_tools 导航插件

2.1、下载源码

进入工作空间 src 目录，克隆插件源码，输入以下指令：

cd ~/ros2_zice/src
# Github 地址
git clone https://github.com/6-robot/wp_map_tools.git
# Gitee 备用地址
git clone https://gitee.com/s-robot/wp_map_tools.git

下载完成之后如下图所示：

可以看到多了一个wp_map_tools文件夹

2.2 安装依赖

进入插件脚本目录，执行 Humble 版本依赖安装脚本，输入以下指令

cd ~/ros2_zice/src/wp_map_tools/scripts/
./install_for_humble.sh

2.3、编译插件

回到工作空间根目录编译,输入以下指令：

cd ~/ros2_zice
colcon build
source install/setup.bash

三、RViz 可视化添加并保存航点

在使用前，我们需要导入地图文件，我们上一期博客已经完成了这个步骤，这里我们不需要再进行了，也就是如下图所示：

3.1、启动航点编辑界面

输入以下指令自动打开 RViz，加载我们之前建好的环境地图：

source install/setup.bash
ros2 launch wp_map_tools add_waypoint_sim.launch.py

如下图所示:

此时我们可以在Rviz2工具栏的右边，可以看到新增了一个Add Waypoint按钮。

单击Add Waypoint按钮，就可以在地图上添加航点，在RViz2窗口里的地图找到要添加航点的位置，单击鼠标左键并按住不放，会出现一个绿色箭头，单击鼠标左键并按住不放，会出现一个绿色箭头，箭头的尾部就是所添加航点的坐标位置。拖动鼠标，绿色箭头会跟着旋转，箭头指向就是航点的朝向。,放置成功后会如下图所示（这里我放了三个）

左右的绿色箭头：按住拖动可以水平平移航点的位置。

上下的红色箭头：按住拖动可以垂直平移航点的位置。

粉色圆点 + 蓝色数字：这是航点的核心目标位置，以及航迹点的序号。

**粉色大箭头：**就是机器人到达这个航点后，车头要对准的方向。

3.2、保存航点文件

新开终端，执行航点保存命令，输入以下指令：

source install/setup.bash
ros2 run wp_map_tools wp_saver

执行完毕后，在用户的主文件夹下会生成一个名为"waypoints.yaml"的文件，如下图所示：

打开此文件就可以看到航点的信息，如下所示：

四、启动导航服务

在wp_map_tools软件包中准备了两个节点可以简化启动导航服务这个调用过程： wp_edit_node节点。这个节点会从主文件夹下的"waypoints.yaml"文件中获取之前保存的航点信息，供其他节点查询使用。

wp_navi_server节点。这个节点会从话题"/waterplus/navi_waypoint"中获取导航的目标航点名称，然后从wp_edit_node节点查询该航点的坐标和朝向，

接着调用NAV2的原生导航接口，完成导航任务。

导航完成后，会向话题"/waterplus/navi_result"发送信息 "navi done"提示导航已经完成。

有了这两个节点，就可以通过话题通讯完成导航任务，下面将介绍如何使用这两个节点。

编写多航点导航 Launch 文件

在之前创建的 nav_pkg 功能包 launch 目录下，新建 waypoint_nav.launch.py，一键启动 NAV2 导航、RViz、航点管理节点、导航服务节点。

该启动文件核心作用：

加载 wpr_simulation2/maps 下的栅格地图；

引入 NAV2 官方 bringup_launch.py，启动全套导航服务；

启动 wp_edit_node 加载本地航点文件，提供航点查询服务；

启动 wp_navi_server 对接 NAV2，接收航点指令并自动完成导航；

加载导航专用 RViz 配置，方便查看地图与路径。

全部代码如下所示：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    # 加载地图文件路径
    map_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'maps',
        'map.yaml'
    )

    # 加载NAV2参数配置
    nav_param_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'config',
        'nav2_params.yaml'
    )

    # 找到nav2_bringup官方启动目录
    nav2_launch_dir = os.path.join(
        get_package_share_directory('nav2_bringup'), 
        'launch'
    )

    # 启动NAV2核心导航
    navigation_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([nav2_launch_dir, '/bringup_launch.py']),
        launch_arguments={
            'map': map_file,
            'use_sim_time': 'True',
            'params_file': nav_param_file}.items(),
    )

    # 加载RViz配置
    rviz_file = os.path.join(
        get_package_share_directory('wp_map_tools'), 
        'rviz', 
        'navi.rviz'
    )
    rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_file]
    )

    # 航点编辑节点：加载waypoints.yaml航点文件，提供航点查询服务
    wp_edit_cmd = Node(
        package='wp_map_tools',
        executable='wp_edit_node',
        name='wp_edit_node'
    )

    # 航点导航服务节点：接收话题指令，对接NAV2实现自动导航
    wp_navi_server_cmd = Node(
        package='wp_map_tools',
        executable='wp_navi_server',
        name='wp_navi_server'
    )

    ld = LaunchDescription()
    ld.add_action(navigation_cmd)
    ld.add_action(rviz_cmd)
    ld.add_action(wp_edit_cmd)
    ld.add_action(wp_navi_server_cmd)

    return ld

五、构建航点导航程序

我们要编写一个节点，向话题"/waterplus/navi_waypoint"发送导航目的地的航点名称，激活wp_navi_server节点的导航功能，完成导航任务。

5.1、编写节点代码

在 nav_pkg 软件包下的src 文件夹中，创建 waypoint_navigation.cpp文件，输入以下代码：

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

// 定义节点句柄
std::shared_ptr<rclcpp::Node> node;

// 导航结果回调函数：收到navi done提示到达终点
void ResultCallback(const std_msgs::msg::String::SharedPtr msg)
{
    if(msg->data == "navi done")
    {
      RCLCPP_INFO(node->get_logger(), "✅ 机器人已到达目标航点！");
    }
}

int main(int argc, char** argv)
{
  // 初始化ROS2
  rclcpp::init(argc, argv);
  // 创建节点
  node = std::make_shared<rclcpp::Node>("waypoint_navigation_node");

  // 创建发布器：向/waterplus/navi_waypoint发送航点名称
  auto navigation_pub = node->create_publisher<std_msgs::msg::String>(
      "/waterplus/navi_waypoint", 10);

  // 创建订阅器：监听导航完成结果
  auto result_sub = node->create_subscription<std_msgs::msg::String>(
      "/waterplus/navi_result", 10, ResultCallback);

  // 延时1秒等待导航服务就绪
  rclcpp::sleep_for(std::chrono::milliseconds(1000));

  // 发送航点编号1，让机器人前往1号航点
  std_msgs::msg::String waypoint_msg;
  waypoint_msg.data = "1";
  navigation_pub->publish(waypoint_msg);
  RCLCPP_INFO(node->get_logger(), "🚀 已下发指令，前往航点1");

  // 循环等待回调
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

5.2、修改 CMakeLists.txt 编译配置

因为我们在 nav_pkg 中新增了 C++ 源码节点 waypoint_navigation.cpp ，原来的 CMakeLists.txt 只支持启动文件安装，无法编译 C++ 程序。

所以我们需要对配置文件做三处关键修改：

添加 C++ 编译依赖（rclcpp、std_msgs）。

添加可执行文件编译规则，将 cpp 源码编译成可运行节点。

添加安装规则，让系统能找到编译后的程序。

完整代码如下所示：

cmake_minimum_required(VERSION 3.8)
project(nav_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# ==============================
# 1. 声明功能包所需依赖
# ament_cmake：ROS2编译工具
# rclcpp：C++核心接口
# nav2_bringup、navigation2：NAV2导航依赖
# std_msgs：标准消息类型
# ==============================
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(nav2_bringup REQUIRED)
find_package(navigation2 REQUIRED)
find_package(std_msgs REQUIRED)

# ==============================
# 2. 编译C++多航点导航节点
# 将src/waypoint_navigation.cpp编译成可执行文件
# ==============================
add_executable(waypoint_navigation src/waypoint_navigation.cpp)

# ==============================
# 3. 为可执行文件添加依赖
# 必须声明依赖，否则编译报错
# ==============================
ament_target_dependencies(waypoint_navigation
  rclcpp
  std_msgs
)

# ==============================
# 4. 安装launch文件夹
# 让ros2 launch能找到启动文件
# ==============================
install(
  DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

# ==============================
# 5. 安装C++可执行文件
# 让ros2 run能找到编译后的节点
# ==============================
install(TARGETS
  waypoint_navigation
  DESTINATION lib/${PROJECT_NAME}
)

# 测试配置（默认保留）
if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

5.3、修改 package.xml 依赖

同理package.xml我们也需要添加c++的依赖，全部代码如下所示：

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>nav_pkg</name>
  <version>0.0.0</version>
  <description>NAV2多航点导航功能包</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 导航依赖 -->
  <depend>rclcpp</depend>
  <depend>nav2_bringup</depend>
  <depend>navigation2</depend>

  <!-- C++ 与消息依赖（新增） -->
  <depend>std_msgs</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

六、编译与运行

编译功能包

打开终端，输入以下指令：

cd ~/ros2_zice
colcon build --packages-select nav_pkg
source install/setup.bash

运行多航点导航

1、首先我们启动Gazebo仿真，打开终端，输入以下指令：

ros2 launch wpr_simulation2 robocup_home.launch.py

2、启动导航 + 航点服务

打开另外一个终端，输入以下指令：

ros2 launch nav_pkg waypoint_nav.launch.py

此时窗口中还没有显示机器人模型，需要手动设置一下机器人的初始位置。

我们需要点击此按钮来设置：2D Pose Estimate，如下图所示：

设置好如下图所示：

3、启动自动航点导航

再打开一个终端，输入以下指令：

ros2 run nav_pkg waypoint_navigation

此时节点执行之后，RViz2中便规划出去往航点"1"的路线，机器人按照路线开始移动。如下图所示：

Gazebo仿真软件下，机器人也在同步运行：

机器人导航到目标航点位置后，会调整自己的朝向，直到与航点"1"的标记箭头方向一致。如下图所示：

我们在终端里输入以下指令，让机器人去点位 2

ros2 topic pub /waterplus/navi_waypoint std_msgs/msg/String "{data: '2'}" -1

也就是在话题里发送指令，可以看到机器人往点完2走了，如下图所示：

---

总结

本期博客我们借助 wp_map_tools 插件实现多航点巡航导航实验。