[ROS2] 使用LifecycleNode管理节点起停等状态

有顺序的的启动节点，暂停节点，关闭节点是ROS1的一个痛点。因为在ROS1中节点启动是无序的。ROS1系统设计时并没有考虑节点启动时可能存在的互相依赖。

但在实际生产使用环境中，某些节点能正常工作是需要其他一些节点已经启动了的。

比如：需要定位功能能正常运行，前提是map_server节点已经正常加载地图文件并且已经把地图话题发布出来。

又或者你想从建图功能切到导航功能。在ROS1中，需要先将建图功能的程序杀干净。然后再启动导航程序。在ROS2中，各个节点的状态是可管理的。

在这个场景里，大可让建图程序休眠，而不用杀掉。切换功能时只需要激活相应功能的节点即可。

ROS2中引入了节点生命周期管理的概念，正是为了处理上面描述的问题。这项功能的实现需要做下面两件事情。

继承LifecycleNode 来实现自己的节点
将节点名称注册到Lifecycle Manager 中，由它来管理各个节点的状态

实现一个功能通常需要一些节点互相配合来实现。这样的话，我们可以将某一个功能涉及到的节点使用一个Lifecycle Manager 程序来管理。从而实现了起停一项功能的效果。

ROS2中的节点类型

ROS2中提供了两种节点类型。

Node() 是和ROS1中一样的节点基类
LifecycleNode() 是可管理状态的节点基类

这里详细说说LifecycleNode() 节点类型。
LifecycleNode 类型的节点可以处于下面几种状态：

Unconfigured
Inactive
Active
Finalized

每一种状态下，可以让节点运行不同的代码。

Unconfigured 状态
当一个节点被创建时，它首先会进入到Unconfigured 状态。
该状态下可通过执行onConfigure()切换到Inactive；
该状态下可通过执行onShutdown()切换到Finalized。

Inactive 状态
在这个状态下，将不响应话题，服务，action和参数等数据交互。处于该状态说明节点已经被配置好了。下一步激活就可以正常执行功能代码了。
该状态下可通过执行onCleanup()切换到Unconfigured；
该状态下可通过执行onActivate()切换到active；
该状态下可通过执行onShutdown()切换到Finalized。

Active 状态
该状态是节点的运行状态，可以接受话题消息，响应服务请求和action请求。数据接收到后进行处理然后输出结果。主要的功能代码应该在该状态下执行。
该状态下可通过执行onDeactivate()切换到inactive；
该状态下可通过执行onShutdown()切换到Finalized。

Finalized 状态
该状态是节点待销毁前的一个状态。在该状态下执行destroy() 将释放节点的资源。

下图演示了各个状态之间是如何切换的。

life_cycle_sm

图片来源于：http://design.ros2.org/articles/node_lifecycle.html
图中，蓝色部分是表示节点状态，黄色部分是状态转换需执行的函数。

需要注意的是，LifecycleNode 类型节点目前只可以在C++中使用

从图上可以看出，LifecycleNode 类型节点切换状态是通过执行一系列的函数实现的。这些函数在继承LifecycleNode 类型节点时是需要重新实现的。

这些切换状态的函数接口声明在/opt/ros/galactic/include/rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp文件中。
主要有下面几个：

  
/// Callback function for configure transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_configure(const State & previous_state);

  

/// Callback function for cleanup transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_cleanup(const State & previous_state);

  

/// Callback function for shutdown transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_shutdown(const State & previous_state);

  

/// Callback function for activate transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_activate(const State & previous_state);

  

/// Callback function for deactivate transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_deactivate(const State & previous_state);

  

/// Callback function for errorneous transition

/*

* \return false by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_error(const State & previous_state);

每一个状态转换函数可以返回三种状态：

enum class CallbackReturn : uint8_t

{

SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS,

FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE,

ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR

};

在任何状态下，执行状态转换函数返回错误(ERROR)时，节点都会执行on_error()。如果执行成功则节点转换到Unconfigured状态。如果执行失败则节点转换到Finalized状态。

每一个状态转移函数都有需要实现的功能。
onConfigure()
顾名思义，该函数主要负责配置好节点，包括初始化节点内要使用的资源，读取参数，新建话题发布器订阅器，服务，action等等。

onConfigure运行成功，节点将从Unconfigured切换到Inactive状态。如果运行失败则仍然处于Unconfigured状态。如果运行返回错误则进行错误处理（即onError())。

onCleanup()
主要用于清除节点的状态。使节点内各状态数据恢复到节点刚刚创建的时候。

onCleanup运行成功，节点将从Inactive切换到Unconfigured状态。如果运行返回错误则进行错误处理（即onError())。

onActivate()
在该函数里一般做一些节点功能运行前的最后准备工作。比如激活话题订阅器发布器等等。

onActivate运行成功，节点将从Inactive切换到Active状态。如果运行返回错误则进行错误处理（即onError())。

onDeactivate()
在该函数里执行的操作一般与onActivate()相反。比如复位话题订阅器发布器等等。

onDeactivate运行成功，节点将从Active切换到Inactive状态。如果运行返回错误则进行错误处理（即onError())。

onShutdown()
在这里主要执行节点销毁前的一些操作。除了Finalized状态外，其他任何状态下都可以运行该函数使节点状态切换至Finalized状态。

onShutdown运行成功，节点将目前状态切换到Finalized状态。如果运行返回错误则进行错误处理（即onError())。

onError()
任何其他状态转移函数执行错误都将执行这个函数。在这里执行一些异常的应对策略。