ROS2导航实操 —— 路径平滑

一、问题描述

在进行ros2导航时，发现规划器服务节点/planner_server和规划平滑器服务节点/smoother_server都是存在的，但是对应的topic /plan和 /plan_smoothed 在发布导航点时只有前者有路径输出。

二、输出 /plan_smoothed 数据

ros2 导航是采用行为树进行任务调度管理，存在两者导航模式：单点导航 和 多点导航，查看源码可看到：对应的行为树配置分别为：navigate_to_pose_w_replanning_and_recovery.xml、navigate_through_poses_w_replanning_and_recovery.xml。

// 单点导航： navigate_to_pose.cpp
  if (!node->has_parameter("default_nav_to_pose_bt_xml"))
  {
    std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
    node->declare_parameter<std::string>( "default_nav_to_pose_bt_xml", pkg_share_dir + "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
  }
  node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);

// -------------------------------------------------------------------------

// 多点导航： navigate_through_poses.cpp
  if (!node->has_parameter("default_nav_through_poses_bt_xml")) {
    std::string pkg_share_dir = ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
    node->declare_parameter<std::string>("default_nav_through_poses_bt_xml", pkg_share_dir + "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
  }
  node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename);

以 单点导航 (行为树: navigate_to_pose_w_replanning_and_recovery.xml) 为例：
我们可以看到：规划器根据目标点生成路径后，直接传给控制器用于路径跟随进行导航控制了，根本没有经过路径平滑，即行为树没有路径平滑节点进行执行。

那么，需要我们做的在行为树中将规划器输出的黑板条目{plan}，先传给路径平滑器，路径平滑器输出新平滑后的路径黑板新条目{plan_smoothed}，再传给控制器用于路径跟随。

重新执行导航，可以看到有路径平滑的效果(下图: 洋红色为原始路径，绿色为平滑路径)，有一定的平滑效果。

ros2 默认的导航路径平滑采用的是 拉普拉斯平滑，只与当前点和前后两点共三点进行平滑，平滑能力有限(如下图，对于这种锯齿型路径，此种平滑无法解决)，如需更好的平滑效果，需要其他方法，如：样条插值、贝塞尔曲线等。

 

三、使用自己的行为树进行平滑

实际项目中，应该创建自己的行为树，那如何加载自己的行为树嘞？

1. 修改配置文件

仍以单点导航为例，根据其源码知道初始化时，需要从参数服务器验证加载default_nav_to_pose_bt_xml参数，那么我们现在，自己的配置中添加此变量

...
bt_navigator:
  ros__parameters:
    use_sim_time: True
    ...
    default_nav_to_pose_bt_xml: ''  # 占位符，将被替换
    ...
 ...

2. 创建自定义行为树

拷贝一份原生的 navigate_to_pose_w_replanning_and_recovery.xml 到自己的工程目录下并重命名，如：my_navigation2/behavior_trees/my_nav_to_pose_bt.xml，其内容参考步骤二。

3. 修改启动文件

修改自己的launch启动文件，如：start_navi2_launch.py ，然后就可以使用自己的行为树快乐的导航玩耍了~

    ...
    my_navigation2_dir = get_package_share_directory('my_navigation2')
		# 为单点导航类型定义行为树路径
    nav_to_pose_bt_path = PathJoinSubstitution([
        my_navigation2_dir,
        'behavior_trees',
        'my_nav_to_pose_bt.xml'
    ])
    
    # 创建重写的YAML配置，添加行为树参数
    param_substitutions = {
        'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
    }
    ...