ARTICLE · 1266699
Nav2 源码(1):LifecycleManager 是怎么工作的?
Nav2 源码(1):LifecycleManager 是怎么工作的?一，背景 本来是想进一步学习 Ros2 的应用，然后就找了一个入口：Nav2。在跟着 AI 学习 Nav2 的过程中，发现一个问题：AI 告诉我怎么做，我就怎么做，除了问题，也不知道怎么回事，还得问 AI。相当于我给 AI 当助手，我啥也没得到。 后来 AI 让我看全局规划的算法源码，我去看了，没看懂。所以之前水了一篇 基础算法（1）：开篇 -- 从冒泡排序开始，这是重新学习算法的开始，后面还会继续水。虽然全局规划没看懂，但是也没白折腾。这期间，我就很想知道这个算法是怎么被 Nav2 用起来的。然后在 Nav2 的源码，Ros2 的源码，还有 TurtleBot3 启动脚本中翻找，终于发现了一点端倪。 二，从一个 Nav2 模块开始 AI 起初让我看的代码是 nav2_navfn_planner 模块，这个东西在 Nav2 的源码包里可以看到。 简单介绍一下，这个模块是 Nav2 默认的全局路线规划模块，主要用 Dijkstra 和 A-star 做全局路线规划。 以下是我逐步跑到 lifecycle-manager 的过程，其实挺随机的，虽然看着像流水账，但是挺重要的。 图 1 
从图中可以看出来，这个模块会生成一个叫 nav2_navfn_planner 的 so。那么它怎么被加载的呢？于是接着到处搜。 图 2 
在一个叫做 nav2_params.yaml 的配置文件中，有对 planner_server 的配置项，看到对 nav2_navfn_planner 的引用。 
图 4 
从这个文件的搜索结果来看，只有这一处引用了 nav2_params.yaml。这处引用，意思是声明一个变量 params_file，默认值是"目录+nav2_params.yaml"。在这个 py 文件中，引用 params_file 的地方有挺多的： 图 5 
把 params_file 作为参数传递给其他的 launch 文件。好像有点死胡同了。 之前做过 gazeboo 模拟算路和导航，用到 tb3_simulation_launch.py 文件，这个文件中就引用了 bringup_launch.py： 图 6 
然后就又回到 bringup_launch.py 这个文件里翻翻它的代码配置，发现一处引用： 图 7 
在这里，还会把 params_file 作为参数，传给 navigation_launch.py，进而引用 navigation_launch.py 文件，执行 navigation_launch.py 之中定义的各种操作。 然后翻了一下 navigation_launch.py 文件： 
在这里看到一个熟悉的家伙：nav2_lifecycle_manager。 之所以对它印象比较深，是因为之前做实验的时候，遇到错误，gazeboo 模拟器中的机器人不能做路线规划，然后求助 AI，AI 帮忙排查的时候，重点提到过这个玩意儿，给出各种操作指导，最终问题解决了，但是稀里糊涂。 所以：起初是为了看 nav2_navfn_planner 源码，学习路线规划，后来变成了看 nav2_lifecycle_manager 的代码。 很随机。 三，nav2_lifecycle_manager 启动 首先，还是从 launch 文件入手，看看给 nav2_lifecycle_manager 传了什么参数： 代码块 1 这里第三个参数，我比较好奇，很想知道它是个啥，然后搜了一下： 代码块 2 是个字符串数组，而且，注意数组的名字：lifecycle_nodes。它是 lifecycle 节点数组，不是普通的 Node。在 Ros2 框架中，这两种节点是不同的。 做一个 lifecycle-manager 启动的小总结： 
2，看一下 main.cpp 很简单，就构造一个 nav2_lifecycle_manager::LifecycleManager，然后就 spin()。那我们就看看 LifecycleManager 这个 class。 3，LifecycleManager 类定义 这里只有第 1 行代码，我认为应该提一下，LifecycleManager 的基类是 rclcpp::Node。有时候，去搜 AI，它可能告诉你基类是 LifecycleNode.从这一点来看，AI 虽然很好，但是不能完全用它来写代码，可能会被带偏。古法编程永远不过时。 4，LifecycleManager 构造函数 首先说明： 除了构造函数是 public 之外，其余成员函数和成员变量全是 protected。 没太理解这么做的原因。 从 protected 猜测，成员函数和服务主要是以对 nav2 内部模块使用为主，对外虽然提供 service，但是也是调试等作用，属于次要目标。 第 11 行：这里有个 bond_timeout，这里利用了一个 Bond 包，它是 Ros2 的一个功能包，提供一个节点到节点的心跳检测功能。虽然 Ros2 Qos 可以配置心跳，但是，Qos 的心跳属于 RTPS 协议，应用层不能直接使用它。Bond 不在 Ros2 源码包之内，如果需要看源码，需要独立下载。这个 Bond 提供的心跳功能在 Ros2 中没少使用，要不是看 LifecycleManager，还真不知道有这东西。 第 33 行：创建一个 callback-group。 create_callback_group() 的第 1 个参数是 MutuallyExclusive。就是说，这个 callback-group 里的回调不能并发执行。后面提到的 2 个 srv 和 1 个 timer，其实都会加入到这里。如果并发执行，会比较乱。 第 2 个参数，是 false。说明这个 callback-group 创建之后，不会自动把当前 Node 加入到 callback-group 之中。需要手工管理。默认情况下，第二个参数是 true，创建之后，会把当前 Node 加入到 callback-group 之中。 第 36 ~ 40 行：创建一个 service，它的名字由 get_name() + "/manage_nodes" 组成。get_name() 是返回当前节点的名称，可能是默认，也可能是通过 launch 文件传进来。lifecycle-manager 的名称从 navigation_launch.py 文件中传入的是 lifecycle_manager_navigation，所以，全称是：“/lifecycle_manager_navigation/manage_nodes”。这个可以用 ros2 service list 看到。当然，要是配置成“zhangsan” 也不是不行。 这个 service 的主要功能是节点内部使用，比如行为树 BT 会发出一个命令给 lifecycle-manager，lifecycle-manager 触发回调函数 managerCallback() 执行。命令有 startup，pause 等，对管理的节点执行对应的操作。这个命令的执行，是针对所有节点，不是其中某一个。 此外，也可以通过命令行给服务发送命令，但是不建议直接干预。 要想知道为啥是 "{command: 1}"，就可以查一下 ManageLifecycleNodes 的定义。 第 42 ~ 46 行：创建另一个 service，命名规则同上。功能是查询 lifecycle-manager 状态，返回 true 或者 false。true 就是正常。 第 70 ~ 87 行：这段代码有点绕，也挺巧的。花了好长时间我才略窥一二。让我写，我写不出来。我把它单独贴出来，整理一下格式再看。 第 5 ~ 6 行：对所有节点执行 changeStateForAllNodes() 函数，先切换到 TRANSITION_CONFIGURE 状态，然后再切换到 TRANSITION_ACTIVATE 状态。到 active 状态的时候，被监控的节点就已经激活了，可以正常接收请求，开始它的服务了。这是 lifecycle-node 的工作机制。 第 13 行：创建一个心跳监视器。它的功能是对所有被监控的节点做心跳监控，定期检测心跳是否断开。如果断开，会执行 reset() 操作。reset() 操作会把所有的节点再拉起来。 以上全部，就是 lifecycle-manager 激活节点的过程了。其他的 pause，reset 这些，就不展开啦，就是记录、更新 lifecycle-node 的状态。最主要的部分我觉得这些已经够了。 后面应该是 planner-server 了，如果有可能的话，应该还会和 lifecycle-manager 有联动。 就这样
那么，nav2_params.yaml 又是在哪里引用的呢？
图 3
从搜索结果来看，其实有不少引用，我们选择第一个：bringup_launch.py。
ComposableNode(package='nav2_lifecycle_manager',plugin='nav2_lifecycle_manager::LifecycleManager',name='lifecycle_manager_navigation',parameters=[{'use_sim_time': use_sim_time,'autostart': autostart,'node_names': lifecycle_nodes}]),
lifecycle_nodes = ['controller_server','smoother_server','planner_server','behavior_server','bt_navigator','waypoint_follower','velocity_smoother']
params_file.ymal 文件中，会对 Nav2 的各个模块做配置，然后把这个文件作为参数传递个 launch 文件。launch 文件运行时，会对 launch 文件和 yaml 文件做解析，把各个模块的参数的隶属关系做映射。启动 lifecycle-manager 时，会把 lifecycle_nodes 数组传给它，它对数组的节点做管理。被管理的节点，会根据自己的配置，加载模块。
例如：lifecycle-manager 会管理 planner_server 模块，planner_server 模块启动时，会加载 nav2_navfn_planner 插件。
以上并不是 Nav2 启动的全部，因为行为树和其他节点这里没有提到。但是也可以管中窥豹了。
四，一个 lifecycle-manager 的误区
从名字上来看，起初我以为 lifecycle-manager 会负责启动，关闭，清理被监管的节点。后来看源码的时候，发现好像不对（感觉不对的地方，等会儿说说），然后翻了一下 navigation_launch.py 文件，有如下内容：
代码块 3
load_nodes = GroupAction(condition=IfCondition(PythonExpression(['not ', use_composition])),actions=[/// 省略部分代码Node(package='nav2_planner',executable='planner_server',name='planner_server',output='screen',respawn=use_respawn,respawn_delay=2.0,parameters=[configured_params],arguments=['--ros-args', '--log-level', log_level],remappings=remappings),/// 省略部分代码])///// 以下是启动模块部分# Create the launch description and populateld = LaunchDescription()# Set environment variablesld.add_action(stdout_linebuf_envvar)# Declare the launch optionsld.add_action(declare_namespace_cmd)ld.add_action(declare_use_sim_time_cmd)ld.add_action(declare_params_file_cmd)ld.add_action(declare_autostart_cmd)ld.add_action(declare_use_composition_cmd)ld.add_action(declare_container_name_cmd)ld.add_action(declare_use_respawn_cmd)ld.add_action(declare_log_level_cmd)# Add the actions to launch all of the navigation nodesld.add_action(load_nodes)ld.add_action(load_composable_nodes)
按 launch 文件内容来看，planner_server 是作为一个独立进程启动的，似乎并不是由 lifecycle-manager 启动，和最开始的理解不一样。
然后搜了一下 AI，确认 ：
lifecycle-manager 虽然管理节点，但是并不负责启动。
启动和管理分工如下：
1）launch：创建模块进程，例如 planner_server。创建时，模块的状态处于 unconfigured。
2）lifecycle-manager：所谓的“manager”，主要是对 lifecycle 节点的状态转换做处理。例如：unconfigured -> inactive -> active。
五，lifecycle-manager 源码
图 9
模块代码只有 2 个 h 和 3 个 cpp 文件。其中，lifecycle_manager_client.hpp 文件的内容在 lifecycle-manager 中并没有用到，接下来我们就不再花精力给它。
1，我们看一下 CMakeLists.txt 文件：
代码块 4
cmake_minimum_required(VERSION 3.5)project(nav2_lifecycle_manager)set(library_name ${PROJECT_NAME}_core)add_library(${library_name} SHAREDsrc/lifecycle_manager.cppsrc/lifecycle_manager_client.cpp)add_executable(lifecycle_managersrc/main.cpp)target_link_libraries(lifecycle_manager${library_name})rclcpp_components_register_nodes(${library_name}"nav2_lifecycle_manager::LifecycleManager")/// 省略部分代码ament_package()
第 6 ~ 9 行：生成一个共享库，依赖 2 个 cpp 文件；
第 11 ~ 17 行：生成一个可执行文件，依赖 1 个 cpp 文件和 1 个共享库；
代码块 5
#include<memory>#include"nav2_lifecycle_manager/lifecycle_manager.hpp"#include"rclcpp/rclcpp.hpp"intmain(int argc, char ** argv){rclcpp::init(argc, argv);auto node = std::make_shared<nav2_lifecycle_manager::LifecycleManager>();rclcpp::spin(node);rclcpp::shutdown();return 0;}
代码块 6
class LifecycleManager : public rclcpp::Node{public:/*** @brief A constructor for nav2_lifecycle_manager::LifecycleManager* @param options Additional options to control creation of the node.*/explicitLifecycleManager(const rclcpp::NodeOptions & options= rclcpp::NodeOptions());/*** @brief A destructor for nav2_lifecycle_manager::LifecycleManager*/~LifecycleManager();/// 省略部分代码}
代码块 7
LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options): Node("lifecycle_manager", options),diagnostics_updater_(this){RCLCPP_INFO(get_logger(), "Creating");// The list of names is parameterized, allowing this module to be used with a different set// of nodesdeclare_parameter("node_names", rclcpp::PARAMETER_STRING_ARRAY);declare_parameter("autostart", rclcpp::ParameterValue(false));declare_parameter("bond_timeout", 4.0);declare_parameter("bond_respawn_max_duration", 10.0);declare_parameter("attempt_respawn_reconnection", true);registerRclPreshutdownCallback();node_names_ = get_parameter("node_names").as_string_array();get_parameter("autostart", autostart_);double bond_timeout_s;get_parameter("bond_timeout", bond_timeout_s);bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::duration<double>(bond_timeout_s));double respawn_timeout_s;get_parameter("bond_respawn_max_duration", respawn_timeout_s);bond_respawn_max_duration_= rclcpp::Duration::from_seconds(respawn_timeout_s);get_parameter("attempt_respawn_reconnection",attempt_respawn_reconnection_);callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);manager_srv_ = create_service<ManageLifecycleNodes>(get_name() + std::string("/manage_nodes"),std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3),rclcpp::ServicesQoS().get_rmw_qos_profile(),callback_group_);is_active_srv_ = create_service<std_srvs::srv::Trigger>(get_name() + std::string("/is_active"),std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3),rclcpp::ServicesQoS().get_rmw_qos_profile(),callback_group_);transition_state_map_[Transition::TRANSITION_CONFIGURE]= State::PRIMARY_STATE_INACTIVE;transition_state_map_[Transition::TRANSITION_CLEANUP]= State::PRIMARY_STATE_UNCONFIGURED;transition_state_map_[Transition::TRANSITION_ACTIVATE]= State::PRIMARY_STATE_ACTIVE;transition_state_map_[Transition::TRANSITION_DEACTIVATE]= State::PRIMARY_STATE_INACTIVE;transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN]= State::PRIMARY_STATE_FINALIZED;transition_label_map_[Transition::TRANSITION_CONFIGURE]= std::string("Configuring ");transition_label_map_[Transition::TRANSITION_CLEANUP]= std::string("Cleaning up ");transition_label_map_[Transition::TRANSITION_ACTIVATE]= std::string("Activating ");transition_label_map_[Transition::TRANSITION_DEACTIVATE]= std::string("Deactivating ");transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN]= std::string("Shutting down ");init_timer_ = this->create_wall_timer(0s,[this]() -> void {init_timer_->cancel();createLifecycleServiceClients();if(autostart_) {init_timer_ = this->create_wall_timer(0s,[this]() -> void {init_timer_->cancel();startup();},callback_group_);}auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();executor->add_callback_group(callback_group_, get_node_base_interface());service_thread_ = std::make_unique<nav2_util::NodeThread>(executor);});diagnostics_updater_.setHardwareID("Nav2");diagnostics_updater_.add("Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic);}
ros2 service call /lifecycle_manager_navigation/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 1}"代码块 8
init_timer_ = this->create_wall_timer(0s,[this]() -> void{init_timer_->cancel();createLifecycleServiceClients();if (autostart_){init_timer_ = this->create_wall_timer(0s,[this]() -> void{init_timer_->cancel();startup();},callback_group_);}auto executor= std::make_shared<rclcpp::executors::SingleThreadedExecutor>();executor->add_callback_group(callback_group_,get_node_base_interface());service_thread_= std::make_unique<nav2_util::NodeThread>(executor);});
create_wall_timer() 函数的第三个个参数是一个 callback-group，默认值是 nullptr。如果不指定第三个构造函数，那么定时器创建完毕之后，会 add 到 Node 的默认 callback-group。
构造函数结束之后，main.cpp 中的rclcpp::spin() 会触发默认 callback-group 里的这个 timer。
触发之后，会调用 timer 的回调。
这个 timer 的回调里，先是取消定时器，然后重新定义一个定时器，并且创建一个新的 executor 和之前创建的 callback-group 绑定，不在把定时器放在默认 callback-group 中。然后创建新的线程 service_thread_ ，新的线程里轮询这个 callback-group。
下次 rclcpp::spin() 轮询的时候，一样会触发新的 callback-group 中的新 timer。不过下次 spin() 不是 main.cpp 里的了，是 service_thread_ 里的。然后，在这次轮询中，触发新的定时器的回调，里面一样取消定时器，执行 startup()，激活所有的监控节点。
这里单独创建一个线程，从主线程脱离出来，还有一个好处，就是和其他的回调分开，比如，导航过程中的 TF，map 等模块，避免竞争。
真了不起，lifecycle-manager 的作者对 ros2 的框架了如指掌。这种操作我是想不到。还需要继续学习。
这代码非常的绕，得熟悉 create_wall_timer() 函数干了啥，Node 构造函数干了啥，callback-group 里面有啥，rclcpp::spin() 函数又是怎么激活 timer 的，不然，，真的不会理解这段代码。
幸亏我之前这些代码真的都看过，能理解一些，不然，就算让 AI 解答，都不会理解。
古法编程万岁！
这段代码真的精妙。它把初始化操作竟然融入到 spin() 的逻辑中，然后在 main.cpp 的医用层完全无感，也不用在 class 内部再单独定义函数。不过还是太绕了，要是我来做，我会把这些操作拆分到 2 个独立的函数，逻辑清晰，好维护。
嗯。。。好像不理解这段代码，也不影响基于 Nav2 做开发
5，激活被管理节点
lifecycle-node 有自己状态机，节点启动阶段，处于 unconfigered 状态，需要外部激活，才会切换到 inactive，乃至 active 状态。
我们之前提到过的服务 manager_srv_，它的名字是
get_name() + std::string("/manage_nodes")这个服务接收外部的命令，然后执行对应的操作。执行命令的函数是服务的回调，上面的代码有提到：
代码块 9
voidLifecycleManager::managerCallback(const std::shared_ptr<rmw_request_id_t>/*request_header*/,const std::shared_ptr<ManageLifecycleNodes::Request> request,std::shared_ptr<ManageLifecycleNodes::Response> response){switch (request->command) {case ManageLifecycleNodes::Request::STARTUP:response->success = startup();break;case ManageLifecycleNodes::Request::RESET:response->success = reset();break;case ManageLifecycleNodes::Request::SHUTDOWN:response->success = shutdown();break;case ManageLifecycleNodes::Request::PAUSE:response->success = pause();break;case ManageLifecycleNodes::Request::RESUME:response->success = resume();break;}}
第 9 行：startup() 这个函数看起来有点眼熟。没错，它在 代码块 8的第 16 行出现过，它是定时器的回调。
也就是说，startup() 函数要么在定时器内，由 lifecycle-manager 触发，要么接收外部命令，在服务的回调里触发。到这里，我们看到两种方式。其实，还有。
代码块 10
bool LifecycleManager::startup(){message("Starting managed nodes bringup...");if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||!changeStateForAllNodes(Transition::TRANSITION_ACTIVATE)){RCLCPP_ERROR(get_logger(), "Failed to bring up all requested nodes. Aborting bringup.");return false;}message("Managed nodes are active");system_active_ = true;createBondTimer();return true;}
