实现navigation2导航多控制器实时切换功能

实现navigation2导航多控制器实时切换功能

机器人实际在导航过程中，常常需要执行各种各样的控制算法以应对不同的场景，例如：某些路段要完全遵循全局路线不绕过动态障碍物，某些路段想要能实时避开障碍物，与充电座对接，进出电梯等。这就需要机器人能在不同场景切换执行对应的算法插件。

如果有比较熟悉navigation2的代码的话，可以知道navigation2本身就自带控制器或者规划器实时切换功能，只不过在官方教程文档(https://navigation.ros.org/)里并没有明确的写出来。只能看到这么一段描述：

The planner and controller servers host a map of one or multiple plugins wherein a certain plugin will be used for a certain environment, scenario, or task. For instance, the controller server can have a plugin for following a path when in long corridors to stay in the middle of the aisle, and then another plugin for avoiding dynamic obstacles in a crowded place. Selecting which planning algorithm to execute based on the robot’s task can be done through the behavior tree of your navigation system or application server.

规划器或控制器服务器托管1至多个算法插件。可以通过导航系统或应用服务器的行为树来根据机器人的任务选择要执行的算法。

本教程将指导如何配置实现以及相关源码分析的方式，讲解实现导航多控制器实时切换功能。

使用的源码版本为https://github.com/ros-planning/navigation2.git主分支，其它版本也大差不差。

step 1 controller server参数配置

参考配置指南页面：https://navigation.ros.org/configuration/packages/configuring-controller-server.html

看controller_plugins这个参数：

controller_plugins

Type	Default
vector<string’>	[‘FollowPath’]

• Description

List of mapped names for controller plugins for processing requests and parameters.

• Note

Each plugin namespace defined in this list needs to have a plugin parameter defining the type of plugin to be loaded in the namespace.

Example:

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"

这是一个vector容器类型的参数，用于存储控制器插件的映射名字符串。

默认只有‘FollowPath’一个映射名，如果要增加控制器，则往该列表里添加新的字符串，且此列表中定义的每个插件命名空间都需要有一个plugin参数来定义要在命名空间中加载的控制器插件。

举例要加一个叫做'ChargerDocking'的对接充电座控制器，那可以这样配置：

 controller_server:
   ros__parameters:
     controller_plugins: ["FollowPath", "Docking"]
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
     Docking:
       plugin: "ChargerDocking"

源码分析：

在navigation2/nav2_controller/src/controller_server.cpp

程序在on_configure()函数中，首先读出controller_plugins参数的值到controller_ids_，然后加载出全部的控制器插件，再执行一遍每个控制器的初始化。

并将控制器指针与controller_ids_内的字符串映射，存到controllers_容器中（这是一个std::unordered_map对象）。

并在on_activate()函数中激活，

机器人在运行过程中，就是在computeControl()函数中周期调用computeAndPublishVelocity()函数，执行对应控制器程序的computeVelocityCommands()得到实时控制速度，并发布。

可以看到，在周期执行计算控制速度的时候，只需去改变current_controller_的值，就可以选择执行对应的控制器插件的computeVelocityCommands()，来实现切换控制器算法的效果。

current_controller_是怎么修改的？

navogation2中每个大模块的调用，都是基于ROS2 actions实现的。对actions不了解？请先移步这个地址进行学习：Understanding actions — ROS 2 Documentation: Humble documentation

computeControl()就是nav2_controller action server的回调函数。

看看接口是怎么定义的，在navigation2/nav2_msgs/action/FollowPath.action

可以看到#goal definition请求构造中有string controller_id

当收到action client发来的请求时，执行computeControl()回调就会读取controller_id来更新current_controller_。

也会周期执行updateGlobalPath()判断是否有新的请求来更新current_controller_。

是哪个节点在充当action client向controller server发送请求的呢？

看下navigation2的框图！

Controller Server是与BT Navigator Server直接通信的，对！就是基于导航行为树。

step 2 行为树文件配置

（如果对导航行为树功能还不了解或者不熟悉的话，建议先移步nav2 behavior trees教程：Nav2 Behavior Trees — Nav2 1.0.0 documentation (ros.org) 和behaviortree_cpp_v3教程：https://www.behaviortree.dev/docs/3.8/intro）

拿navigation2最基础的行为树文件来改，navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

上图为navigate_to_pose_w_replanning_and_recovery这个行为树文件导航部分子树的树形图。画红圈的部分就是follow_path_action节点。

xml文件里为

follow_path_action节点实现就是controller_server FollowPath的action client。

可以看到controller_id这个参数默认只能是写死的，文件中为"FollowPath"。

要把它改成可以实时切换，需要另外一个行为树节点插件：controller_selector_node 。这个节点插件在现成的行为树文件中都没有被用到，但可以在nav2_behavior_tree这个包中找到。使用教程在：https://navigation.ros.org/configuration/packages/bt-plugins/actions/ControllerSelector.html

把它加到navigate_to_pose_w_replanning_and_recovery这个行为树里，可以这样修改：

        <RecoveryNode number_of_retries="1" name="FollowPath">
          <PipelineSequence>
            <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          </PipelineSequence>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>

运行导航后，就可以通过/controller_selector这个topic发布消息来切换控制器，达到实时切换的效果。

源码分析：

接着我们分析一下follow_path_action和controller_selector_node这两个行为树插件的源码。

打开navigation2/nav2_behavior_tree/plugins/action/follow_path_action.cpp

这个插件的源码非常简单，但它继承的是BtActionNode这个模板类。BtActionNode其实实现的就是导航行为树里通用的ros2 action client，这里不去细讲，感兴趣的自己看源码。

继续看follow_path_action源码。它其实做的就是行为树每次tick的时候去读输入参数，然后对比上一次tick时读到的输入参数。如果有更新，就将goal_updated_赋值为true，这样action client就会发送新的请求让controller_server处理。

然后看一下navigation2/nav2_behavior_tree/plugins/action/controller_selector_node.cpp

也是很简单，创建一个topic订阅，接收消息，更新last_selected_controller_然后输出到"selected_controller"

我们在行为树文件中的更改，将ControllerSelector和FollowPath两个子节点挂载PipelineSequence节点下，这样就实现了两个节点一直轮询执行的效果。ControllerSelector输出的selected_controller让FollowPath读取，从而进行控制器切换。

		  <PipelineSequence>
            <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          </PipelineSequence>

结语

通过前文描述的简单修改，就可以实现导航多控制器实时切换的功能，通过/controller_selector这个topic发布消息来选择要执行的控制算法。应用程序中要通过什么样的方式去发布controller_selector topic，就看自己的需求了。

本文旨在通过源码分析的方式，让大家更深入了解navigation2的运行流程。如果对你有帮助的话，那就请我喝杯咖啡吧~