Navi2实现自定义Planner以及过程中的经验

路径规划
, ,

什么是Navigation2 ？

Navigation2（Nav2）是ROS 2机器人平台生态下官方开发与维护的自主导航（移动机器人定位与路径规划）系统。它让移动机器人可以实现类似“给定目标点，自动避障、去往指定位置”的能力。
简而言之，这是适配ROS2的一个导航框架，可以参考框架进行自定义开发，部署自己的导航算法。

环境要求：

  • Ubuntu 22.04
  • ROS2 Humble
  • Navigation 2
  • Gazebo

实现流程

我在尝试自定义Planner的过程中，尝试将自己的全局路径规划算法部署到Navigation2 中并在Gazebo中配合机器人仿真展示出来。

根据官方文档（地址：）
一共需要继承五个纯虚函数。如下图所示[1]
Nav2 functions

但是在实现的开发过程中，主要关注在configure()函数和createPlan()函数的实现上。官方存在一个demo（地址：https://github.com/ros-navigation/navigation2_tutorials/blob/rolling/nav2_straightline_planner/src/straight_line_planner.cpp）

此外，给出以我自己实现的代码设置为例：

  • nav2_self_planner.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
#ifndef NAV2_SELF_PLANNER__NAV2_SELF_PLANNER_HPP_
#define NAV2_SELF_PLANNER__NAV2_SELF_PLANNER_HPP_
#include <memory>
#include <string>
// 消息接口
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav_msgs/msg/path.hpp"

#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp" //基类 base_type
#include "nav2_util/lifecycle_node.hpp" // rclcpp的一个子类
#include "nav2_util/robot_utils.hpp" //常用工具类


namespace nav2_self_planner {
// 自定义导航规划器类 继承基类（抽象类），纯虚函数
class CustomAstarPlanner : public nav2_core::GlobalPlanner {
public:
  CustomPlanner() = default;
  ~CustomPlanner() = default;
  // 插件配置方法
  void configure(
      const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name,
      std::shared_ptr<tf2_ros::Buffer> tf,
      std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
  // 插件清理方法
  void cleanup() override;
  // 插件激活方法
  void activate() override;
  // 插件停用方法
  void deactivate() override;
  // 为给定的起始和目标位姿创建路径的方法
  nav_msgs::msg::Path
  // 根据官方文档，这里就两个参数
  createPlan(
    const geometry_msgs::msg::PoseStamped &start,
    const geometry_msgs::msg::PoseStamped &goal) override;

private:
  // 坐标变换缓存指针，可用于查询坐标关系
  std::shared_ptr<tf2_ros::Buffer> tf_;
  // 节点指针
  nav2_util::LifecycleNode::SharedPtr node_;
  // 全局代价地图
  nav2_costmap_2d::Costmap2D *costmap_;
  // 全局代价地图的坐标系
  std::string global_frame_, name_;
  // 其他传入参数
  double parameter1;
  double parameter2;
};

} // namespace nav2_self_planner

#endif //
  • nav2_self_planner.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
#include "nav2_self_planner/nav2_self_planner.hpp"  
#include <queue>  
#include <unordered_map> 
#include <unordered_set>  
#include <cmath> 
#include <algorithm> 
#include <vector>

// nav2工具  
#include "nav2_util/node_utils.hpp"  

void CustomAstarPlanner::configure(  
      const rclcpp_lifecycle::LifecycleNode::WeakPtr &amp;parent, std::string name,  
      std::shared_ptr&lt;tf2_ros::Buffer&gt; tf,  
      std::shared_ptr&lt;nav2_costmap_2d::Costmap2DROS&gt; costmap_ros)  
{  
  node_ = parent.lock();  
  name_ = name;  
  tf_ = tf;  
  costmap_ = costmap_ros-&gt;getCostmap();  
  global_frame_ = costmap_ros-&gt;getGlobalFrameID();  
  // 定义参数并设置参数值
  nav2_util::declare_parameter_if_not_declared(  
      node_, name_ + ".parameter1", rclcpp::ParameterValue(0.1));  
  nav2_util::declare_parameter_if_not_declared(  
      node_, name_ + ".parameter2", rclcpp::ParameterValue(0.5));  
  node_->;get_parameter(name_ + ".parameter1", interpolation_resolution_);  
  node_->;get_parameter(name_ + ".parameter2", safety_distance);  
}  

// 这三个一般保持不写

void CustomAstarPlanner::cleanup()
    {
        RCLCPP_INFO(node_->;get_logger(), "正在清理类型为 CustomPlanner 的插件 %s",name_.c_str());
    }

void CustomAstarPlanner::activate()
{
    RCLCPP_INFO(node_->;get_logger(), "正在激活类型为 CustomPlanner 的插件 %s",name_.c_str());
}

void CustomAstarPlanner::deactivate()
{
    RCLCPP_INFO(node_->;get_logger(), "正在停用类型为 CustomPlanner 的插件 %s",name_.c_str());
}

nav_msgs::msg::Path  
CustomAstarPlanner::createPlan(  
    const geometry_msgs::msg::PoseStamped &amp;start,  
    const geometry_msgs::msg::PoseStamped &amp;goal)  
{  
  自定义全局规划算法的实现代码...
}
 // namespace nav2_self_planner  

// 注册并导出插件
#include "pluginlib/class_list_macros.hpp"  
PLUGINLIB_EXPORT_CLASS(nav2_self_planner::CustomPlanner, nav2_core::GlobalPlanner)

其中：

  • 导出插件：
1
2
#include "pluginlib/class_list_macros.hpp"  
PLUGINLIB_EXPORT_CLASS(nav2_self_planner::CustomPlanner, nav2_core::GlobalPlanner)

完成算法对应的.hpp和.cpp文件之后，下一步需要在xml文件里面注册。主要包含三个文件，如下所示：

补充文件：

    1. self_planner_plugin.xml:
1
2
3
4
5
<library path="nav2_self_planner_plugin">
 <class name="nav2_self_planner/CustomAstarPlanner" type="nav2_self_planner::CustomAstarPlanner" base_class_type="nav2_core::GlobalPlanner">
   <description>自定义示例插件，用于生成自定义路径。</description>
 </class>
</library>
    1. package.xml:
1
2
3
<export>
    <nav2_core plugin="${prefix}/self_planner_plugin.xml"/>
<export>
    1. CMakeList.txt:
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
cmake_minimum_required(VERSION 3.8)
project(nav2_self_planner)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav2_core REQUIRED)


# 包含头文件目录
include_directories(include)
# 定义库名称
set(library_name ${PROJECT_NAME}_plugin)
# 创建共享库 加了SHARED 就是动态的库
add_library(${library_name} SHARED  src/nav2_self_planner.cpp)
# 指定库的依赖关系
ament_target_dependencies(${library_name} nav2_core pluginlib)
# 安装库文件到指定目录
install(TARGETS ${library_name}
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
)
# 安装头文件到指定目录
install(DIRECTORY include/
  DESTINATION include/ )
# 导出插件描述文件（这里使用的是动态功能包+描述文件）而不是（当前功能包+描述文件）
pluginlib_export_plugin_description_file(nav2_core self_planner_plugin.xml)

ament_package()

剩下的就是按照官方文档，在navigation2里面的参数设置里替换规划器为自己的即可。

项目感悟和心得

在完成整个项目的过程中，对于小白来说，我认为难的点在于：1.不知道需要哪些第三方库/包。2.此外用到的第三方库/包怎么去查看？

对于前者，建议参考别人实现算法用了什么工具库。你自己大概率也都是使用这些工具库。
比如常使用的库为：

  • <queue>：std::priority_queue (优先队列)

  • <unordered_map>、<unordered_set>：哈希表

  • <vector>：动态数组

  • <cmath>、<algorithm>：数学运算和常用算法

  • ROS/Navigation2 API：在规划器里要用参数声明、Costmap、TF 变换等，就需要包含它们提供的头文件，如

1
2
#include "nav2_util/node_utils.hpp" （参数声明工具）
#include "nav2_costmap_2d/costmap_2d_ros.hpp"

对于第二个问题，比如nav2_utils.hpp，查看里面的函数。有三种方式：

  • 直接ctrl点进去查看函数（最方便）。一般 VSCode、CLion 等 IDE，把光标放在类型或函数上，按 “Go to Definition”或者点击（转到定义）立刻能看到声明、参数和注释。
  • 官网查看

开发技巧

从零去写一个工程很复杂，可以在一些示例工程的基础上进行二次开发。(示例工程地址：https://docs.nav2.org/plugins/index.html)

Navigation2 源码下有其他已有的 planner（如 SmacPlanner、GridBasedPlanner），直接参考它们的实现，复制和改造它们声明的参数和调用方式。