ROS2中的LifecycleNode生命周期节点

本文参考：ros2 node_lifecycle
ros2中加入了一种区别于普通的node节点的管理节点LifecycleNode，这种节点有点像是状态机，会在几种不同的状态中切换。

LifecycleNode 节点的状态主要分为基本状态（Primary States）和切换状态（Transition States）。主要状态是任何节点都可以做相应的任务的稳态，切换状态切换过程中短暂的临时状态。这些中间状态的结果用于指示两个主要状态之间的转换是否成功。

主要状态分为4种：
unconfigured
inactive
active
shutdown

切换状态分为6种：
configuring
activating
deactivating
cleaningup
shuttingdown
ErrorProcessing

关于状态的切换可以看这张图：

Unconfigured

节点实例化之后就会立马进入这个状态，节点出错之后也会返回到这个状态。

有效的状态转换：

• 可以通过configure切换到Inactive状态
• 可以通过shutdown切换到Finalized状态

Inactive

这个状态代表节点目前没有进行任何处理。这个状态的主要目的是让节点进行（重新）配置（比如改变配置参数、增加或删除topic的发布和订阅等）而不在运行时改变它的行为。

在这个状态时，节点不会接受任何运行时间来读取topic、进行数据处理或是回应service的请求等。Inactive状态下不会对收到的任何来自管理话题的数据进行读取或者是处理。数据保留将会取决于为主题配置的QoS策略。同时service请求也不会被回应（对请求者来说，会立马fail）。

有效的状态转换：

• 通过shutdown切换到Finalized
• 通过cleanup切换到Unconfigured
• 通过acticate切换到Active

Active

这个lifecycle节点的主要状态。在这个状态时，节点会进行一些处理、回应service请求、读取和处理数据、产生输出等。

如果这个状态下节点或是系统发生了不能处理的错误，那么节点会切换到ErrorProcessing状态。

有效的状态切换：

• 通过deactivate切换到Inactive
• 通过shutdown切换到Finalized

Finalized

该状态是节点被销毁前会立马就结束的状态。这个状态只会通往销毁。

有效的状态切换：

• 通过destroy被消除分配（deallocated）

Configuring

节点的conConfigure回调函数会被调用，来允许节点加载配置以及进行必要的设置。

节点的配置通常都会涉及到一些在节点的生命周期中必须被执行一次的任务，比如说申请内存、配置不会改变的topic的发布/订阅等等。

节点也会用此来设置一些它的整个生命周期中必须保留的资源（不论是active状态还是inactive状态），比如说topic的发布/订阅器，持续需要的内存空间以及初始的配置参数。

有效的状态切换：

• 如果conConfigure回调函数成功被调用，那么节点会切换到Inactive状态。
• 如果回调函数出现失败代码（需要具体的代码），那么节点就会返回到Unconfigured状态。
• 如果回调函数引发或return了别的值，则节点将转换为ErrorProcessing状态。

CleaningUp

节点的onCleanup回调函数会被调用。函数中应当清除所有状态并返回到与初次创建时一样的状态。

有效的状态切换;

• 如果onCleanup回调函数成功执行，就会被切换到Unconfigured状态。
• 如果执行出错就会切换到ErrorProcessing

Activating

节点的onActivate回调函数会被调用。函数中应当做好开始执行前的最后准备，包括获得只在节点active期间会用到的资源，比如对硬件的访问。理想情况下，不应该在这里执行需要大量时间的准备工作（比如冗长的硬件初始化，我猜测可能是单目SLAM的初始化这种）。

有效的状态切换;

• 如果onActivate回调函数成功执行，就会被切换到Active状态。
• 如果执行出错就会切换到ErrorProcessing

Deactivating

节点的onDeactivate回调函数会被调用。函数中应当执行清除操作以便开始执行，并做onActivate中相反的操作。

有效的状态切换;

• 如果onDeactivate回调函数成功执行，就会被切换到Active状态。
• 如果执行出错就会切换到ErrorProcessing

ShuttingDown

节点的onShutdown回调函数会被调用。函数中应当执行销毁前的必要的清除。该状态应当可以从除了Finalized的所有状态进入，函数中应当将节点返回到初始状态。

有效的状态切换;

• 如果onShutdown回调函数成功执行，就会被切换到Finalized状态。
• 如果执行出错就会切换到ErrorProcessing

ErrorProcessing

该状态是清除所有错误的地方。可以从所有状态进入该状态。如果错误被成功处理，那么会返回到Unconfigured状态，如果没有执行所有的清理，那它必须失败然后切换到Finalized状态并等待销毁。

到ErrorProcessing切换可以是回调函数（或是回调函数中的函数）中生成的错误返回值以及未捕获的异常。

有效的状态切换;

• 如果onError回调函数成功执行，就会被切换到Unconfigured状态。期望的是onError会清除先前状态的所有状态。比如说如果是从Active进入的，那么必须提供onDeactivate和onCleanup的来返回成功。
• 如果执行出错就会切换到Finalized

实例

可以看下面的实例

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>

#include "lifecycle_msgs/msg/transition.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class my_lifecyclenode : public rclcpp_lifecycle::LifecycleNode {
public:
    // 构造函数
    // lifecyclenode 的构造函数都有相同的参数
    explicit my_lifecyclenode(const std::string& node_name, bool intra_process_comms = false)
        : rclcpp_lifecycle::LifecycleNode(node_name,
                                          rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)) {}
    void func() { std::cout << "in func" << std::endl; }

    // on_configure回调函数会在lifecyclenode进入configuring状态时被调用
    // 根据返回值的不同，节点会进入inactive或者停留在unconfigured
    // RANSITION_CALLBACK_SUCCESS transitions to "inactive"
    // RANSITION_CALLBACK_FAILURE transitions to "unconfigured"
    // TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing"
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
        const rclcpp_lifecycle::State&) {
        RCLCPP_INFO(get_logger(), "on_configure() is called.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    // 没啥好说的，参考on_configure回调函数和前文
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State&) {
        RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called.");
        std::this_thread::sleep_for(2s);
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    //
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State&) {
        RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    //
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
        const rclcpp_lifecycle::State&) {
        // 做一些publisher和timer的reset等
        // TODO

        RCUTILS_LOG_INFO_NAMED(get_name(), "on cleanup is called.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
        const rclcpp_lifecycle::State& state) {
        RCUTILS_LOG_INFO_NAMED(get_name(), "on shutdown is called from state %s.", state.label().c_str());
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    auto node = std::make_shared<my_lifecyclenode>("my_lcnode");
    rclcpp::spin(node->get_node_base_interface());
    rclcpp::shutdown();

    return 0;
}

编译好后运行该节点

ros2 run my_package lc_node

这个时候node就处于unconfigure状态
新开一个终端输入

ros2 lifecycle list /my_lcnode

就可以看到my_lcnode节点接下来可以进入什么状态：

- configure [1]
	Start: unconfigured
	Goal: configuring
- shutdown [5]
	Start: unconfigured
	Goal: shuttingdown

再输入

ros2 lifecycle set /my_lcnode configure

就可以将my_lcnode节点设置为configure状态：

[INFO] [1640242411.047224131] [lc_talker]: on_configure() is called.

不过一般在实际中都是利用程序来进行节点的管理，这里附上一个调用lifecyle service来进行节点管理的程序，可以参考着来进行自己的程序编写：

#include <chrono>
#include <memory>
#include <string>
#include <thread>

#include "lifecycle_msgs/msg/state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rcutils/logging_macros.h"

using namespace std::chrono_literals;

static constexpr char const* lifecycle_node = "my_lcnode";

// 每一个lifecycle node都有很多服务与它相关
// 按惯例都用 <node name>/<service name> 的形式
// 在这个demo中我们使用 get_state 和 change_state
// 因此这两个service为:
// lc_talker/get_state
// lc_talker/change_state
static constexpr char const* node_get_state_topic = "lc_talker/get_state";
static constexpr char const* node_change_state_topic = "lc_talker/change_state";

template <typename FutureT, typename WaitTimeT>
std::future_status wait_for_result(FutureT& future, WaitTimeT time_to_wait) {
    auto end = std::chrono::steady_clock::now() + time_to_wait;
    std::chrono::milliseconds wait_period(100);
    std::future_status status = std::future_status::timeout;
    do {
        auto now = std::chrono::steady_clock::now();
        auto time_left = end - now;
        if (time_left <= std::chrono::seconds(0)) {
            break;
        }
        status = future.wait_for((time_left < wait_period) ? time_left : wait_period);
    } while (rclcpp::ok() && status != std::future_status::ready);
    return status;
}

class LifecycleServiceClient : public rclcpp::Node {
public:
    explicit LifecycleServiceClient(const std::string& node_name) : Node(node_name) {}

    void init() {
        // 每个 lifecycle node 都会自动产生几个service来允许外部交互
        // The two main important ones are GetState and ChangeState.
        client_get_state_ = this->create_client<lifecycle_msgs::srv::GetState>(node_get_state_topic);
        client_change_state_ = this->create_client<lifecycle_msgs::srv::ChangeState>(node_change_state_topic);
    }

    /// Requests the current state of the node
    /** 在这个函数中，我们发送一个service请求来询问lc_talker节点的当前状态
     * 如果在给定的 time_out 时间内return了，那就返回当前状态，如果没有就返回一个unknown状态
     * \param time_out 超时的time (秒)
     */
    unsigned int get_state(std::chrono::seconds time_out = 3s) {
        auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();

        if (!client_get_state_->wait_for_service(time_out)) {
            RCLCPP_ERROR(get_logger(), "Service %s is not available.", client_get_state_->get_service_name());
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        }

        // We send the service request for asking the current
        // state of the lc_talker node.
        auto future_result = client_get_state_->async_send_request(request);

        // Let's wait until we have the answer from the node.

        auto future_status = wait_for_result(future_result, time_out);

        // If the request times out, we return an unknown state.
        if (future_status != std::future_status::ready) {
            RCLCPP_ERROR(get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        }

        // We have an succesful answer. So let's print the current state.
        if (future_result.get()) {
            RCLCPP_INFO(get_logger(), "Node %s has current state %s.", lifecycle_node,
                        future_result.get()->current_state.label.c_str());
            return future_result.get()->current_state.id;
        } else {
            RCLCPP_ERROR(get_logger(), "Failed to get current state for node %s", lifecycle_node);
            return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
        }
    }

    /// Invokes a transition
    /**
     * 我们发送一个service请求来表示我们希望根据id "transition" 来进行切换
     * 默认的切换有：
     * - configure
     * - activate
     * - cleanup
     * - shutdown
     * \param transition 区别切换的id
     * \param time_out 超时的time (秒)
     */
    bool change_state(std::uint8_t transition, std::chrono::seconds time_out = 3s) {
        auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
        request->transition.id = transition;

        if (!client_change_state_->wait_for_service(time_out)) {
            RCLCPP_ERROR(get_logger(), "Service %s is not available.", client_change_state_->get_service_name());
            return false;
        }

        // We send the request with the transition we want to invoke.
        auto future_result = client_change_state_->async_send_request(request);

        // Let's wait until we have the answer from the node.
        // If the request times out, we return an unknown state.
        auto future_status = wait_for_result(future_result, time_out);

        if (future_status != std::future_status::ready) {
            RCLCPP_ERROR(get_logger(), "Server time out while getting current state for node %s", lifecycle_node);
            return false;
        }

        // We have an answer, let's print our success.
        if (future_result.get()->success) {
            RCLCPP_INFO(get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition));
            return true;
        } else {
            RCLCPP_WARN(get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
            return false;
        }
    }

private:
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::GetState>> client_get_state_;
    std::shared_ptr<rclcpp::Client<lifecycle_msgs::srv::ChangeState>> client_change_state_;
};

/**
 * 这是一个简单的脚本来依次触发各个服务
 * It starts with configure, activate,
 * deactivate, activate, deactivate,
 * cleanup and finally shutdown
 */
void callee_script(std::shared_ptr<LifecycleServiceClient> lc_client) {
    rclcpp::WallRate time_between_state_changes(0.25); // 4s

    // configure
    {
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // activate
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // deactivate
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // activate it again
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // and deactivate it again
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // we cleanup
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }

    // and finally shutdown
    // Note: We have to be precise here on which shutdown transition id to call
    // We are currently in the unconfigured state and thus have to call
    // TRANSITION_UNCONFIGURED_SHUTDOWN
    {
        time_between_state_changes.sleep();
        if (!rclcpp::ok()) {
            return;
        }
        if (!lc_client->change_state(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)) {
            return;
        }
        if (!lc_client->get_state()) {
            return;
        }
    }
}

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);

    auto lc_client = std::make_shared<LifecycleServiceClient>("lc_client");
    lc_client->init();

    rclcpp::executors::SingleThreadedExecutor exe;
    exe.add_node(lc_client);

    std::shared_future<void> script = std::async(std::launch::async, std::bind(callee_script, lc_client));
    exe.spin_until_future_complete(script);

    rclcpp::shutdown();

    return 0;
}