ROS2 LifeCycleを使ったノード管理

## はじめに


## 1.LifeCycleとは
### 1.1 LifeCycleによってノードの状態を制御できる
ROS1までの通常のノードに加えて、ROS2ではLifeCycle Node(もしくは[Maneged Nodes](https://design.ros2.org/articles/node_lifecycle.html))という仕組みが導入されています。通常のノードは起動した直後から動作し始め、途中で再起動したり、動作を一時停止することはできません。一方LifeCycleNodeは下の画像のような4つの状態を持ちます。


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/4dfd45d3-caf5-b84e-ae92-aff5f821664f.png)
[^1]引用
[^1]:[How to Use ROS 2 Lifecycle Nodes](https://foxglove.dev/blog/how-to-use-ros2-lifecycle-nodes)
- Unconfigured
- Inactive
- Active
- Finalized: 


<details><summary>状態遷移詳細。実はこの他に6つの中間状態を持ち、これらによって柔軟に状態遷移を制御することができる。</summary>


![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/04bfc9b8-f35c-cf4a-4928-ef6fc4d4ccd6.png)
- Configuring: 構成中.ノードの初期化やメモリの確保などを行っている状態
- CleaningUp: 終了処理中
- ShuttingDown: 停止中
- Activating: 活動状態へ移行中。成功するとノードの状態はActiveとなり動作を開始する
- Deactivating: 未活動状態へ移行中。成功するとInactiveとなり動作停止
- ErrorProcessing: エラー処理中


更に詳細については[2],[3]がわかりやすいです。

</details>



### 1.2 使用例
1. ノード間に依存関係がある場合
ノードXが正しく動作するために，ノードYが起動して計算結果をパブリッシュしている必要があるとき、ノードYが動作しているのを確認してから、初めてノードXをactivateするという処理を簡単に作れる。これによってエラーや誤動作をしない仕組みが作れる。
2. リアルタイムでモードを切り替えたりしたい時にも有効
ノードA，ノードBによる処理によって二つのモードがあるとき，activate, deactivateを使うことでうまくモードを切り替えることができる．

    自立走行ロボットを用いて，以下のようなシナリオを想定してみましょう．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/4a5419a9-dea9-4684-888f-c290b7e7c852.png)

    エリア1ではGPSを使った運転モードで，エリア2ではLiDARを使った運転モードで走行したいとします．走行のアルゴリズムは適当なものを仮定すると，外界認識-自己位置推定を行う別々のノードを用意しておいて，切り替え時にactivate, deactivateを行うことで切り替えることができます．
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/2bf81d4d-9da5-434f-a19e-3f22c3023785.png)

    

<!--他にも、あるオンラインの計算に２種類の方法があり、２つの異なるノードA,Bによって別個に実装しているとする。
ノードAは精度が高いが、非常に大きな計算量が必要なアルゴリズムを採用している。一方ノードBは精度は中程度だが計算が軽い。２つのノードを状況に応じて使い分けたいときに、状況を監視するノードを介してA,Bのノードの起動と停止を制御することができる。
-->


## 2.チュートリアル
今回は2つ目の例を用いた実装を行っていきます．先ほどのシナリオを簡略化し，擬似的に再現してみましょう．
以下の画像のようなものを実装してみます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/7e22789d-98ef-49f9-bbed-39df824e43e2.png)




- 始まって20秒間，LowercaseNodeはabcdefg（小文字）をパブリッシュする
- 20秒後，UppercaseNodeはABCDEFG（大文字）をパブリッシュする
- ReverseNodeはサブスクライブした文字列を逆さまにパブリッシュする
- 40秒経つと終了する


### 実装方法
実装コードはこちらにあります．

https://github.com/shiryu-nakano/my_lifecycle_tutorial


今回は自立ロボットを想定しています．ロボットが自分でエリアAからBへ移ったことを認識して，自身でモードを切り替えてくれると嬉しいです．今まで出てきた3つのノードに加えて，状態を監視し，lifecycle nodeの管理を行う``supervisor``というノードを用意します．
このノードが，自身の状態変数としてタイマーを使い．ノードが立ち上がってからの秒数に応じてノードを管理します．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/a4804367-655e-4a8e-9f48-f1707b4542bd.png)


<details><summary>lowercase_node</summary>

```

#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;
using rclcpp_lifecycle::LifecycleNode;
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class LowercaseNode : public LifecycleNode
{
public:
  explicit LowercaseNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : LifecycleNode("lowercase_node", options)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode constructor");
  }

protected:
  CallbackReturn on_configure(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode: on_configure()");
    publisher_ = this->create_publisher<std_msgs::msg::String>("string_topic", 10);
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_activate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode: on_activate()");

    if (publisher_) {
      publisher_->on_activate();
    }

    timer_ = this->create_wall_timer(1s, [this]() {
      auto msg = std::make_shared<std_msgs::msg::String>();
      msg->data = "abcdefg";
      publisher_->publish(*msg);
      RCLCPP_INFO(this->get_logger(), "[LowercaseNode] Publishing: %s", msg->data.c_str());
    });
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode: on_deactivate()");
    if (timer_) {
      timer_->cancel();
      timer_.reset();
    }
    if (publisher_) {
      publisher_->on_deactivate();
    }
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode: on_cleanup()");
    publisher_.reset();
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state)
  {
    RCLCPP_INFO(this->get_logger(), "LowercaseNode: on_shutdown() from [%s]", state.label().c_str());
    return CallbackReturn::SUCCESS;
  }

private:
  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LowercaseNode>();
  rclcpp::executors::SingleThreadedExecutor exe;
  exe.add_node(node->get_node_base_interface());
  exe.spin();
  rclcpp::shutdown();
  return 0;
}


```

</details>


<details><summary>uppercase_node</summary>

```

#include <chrono>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;
using rclcpp_lifecycle::LifecycleNode;
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class UppercaseNode : public LifecycleNode
{
public:
  explicit UppercaseNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : LifecycleNode("uppercase_node", options)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode constructor");
  }

protected:
  CallbackReturn on_configure(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode: on_configure()");
    publisher_ = this->create_publisher<std_msgs::msg::String>("string_topic", 10);
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_activate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode: on_activate()");
    if (publisher_) {
      publisher_->on_activate();
    }
    timer_ = this->create_wall_timer(1s, [this]() {
      auto msg = std::make_shared<std_msgs::msg::String>();
      msg->data = "ABCDEFG";
      publisher_->publish(*msg);
      RCLCPP_INFO(this->get_logger(), "[UppercaseNode] Publishing: %s", msg->data.c_str());
    });
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode: on_deactivate()");
    if (timer_) {
      timer_->cancel();
      timer_.reset();
    }
    if (publisher_) {
      publisher_->on_deactivate();
    }
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode: on_cleanup()");
    publisher_.reset();
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state)
  {
    RCLCPP_INFO(this->get_logger(), "UppercaseNode: on_shutdown() from [%s]", state.label().c_str());
    return CallbackReturn::SUCCESS;
  }

private:
  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<UppercaseNode>();
  rclcpp::executors::SingleThreadedExecutor exe;
  exe.add_node(node->get_node_base_interface());
  exe.spin();
  rclcpp::shutdown();
  return 0;
}


```
</details>

<details><summary>reverse_node</summary>

```
#include <chrono>
#include <memory>
#include <string>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class SakasamaNode : public rclcpp::Node
{
public:
  explicit SakasamaNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("reverse_node", options)
  {
    RCLCPP_INFO(this->get_logger(), "SakasamaNode constructor");

    // サブスクライブ: string_topic
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "string_topic", 10,
      std::bind(&SakasamaNode::callbackString, this, std::placeholders::_1));

    // パブリッシャ: sakasama_topic
    publisher_ = this->create_publisher<std_msgs::msg::String>("sakasama_topic", 10);
  }

private:
  void callbackString(const std_msgs::msg::String::SharedPtr msg)
  {
    auto reversed_str = msg->data;
    std::reverse(reversed_str.begin(), reversed_str.end());

    std_msgs::msg::String out_msg;
    out_msg.data = reversed_str;

    // 受け取ったら即パブリッシュ
    publisher_->publish(out_msg);
    RCLCPP_INFO(this->get_logger(), "[ReverseNode] Reversed Letters: %s", out_msg.data.c_str());
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<SakasamaNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}


```


</details>


<details><summary>supervisor</summary>

```

// supervisor_node.cpp

#include <chrono>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/msg/transition.hpp"

using namespace std::chrono_literals;

class Supervisor : public rclcpp::Node
{
public:
  Supervisor()
  : Node("supervisor")
  {
    RCLCPP_INFO(this->get_logger(), "Supervisor constructor");

    // ライフサイクル制御対象ノード (SakasamaNodeは除く)
    target_nodes_ = {
      "lowercase_node",
      "uppercase_node"
    };

    // /ノード名/change_state クライアントを作成
    for (auto & node_name : target_nodes_) {
      auto client = this->create_client<lifecycle_msgs::srv::ChangeState>(
        node_name + "/change_state"
      );
      clients_[node_name] = client;
    }

    start_time_ = now().seconds();

    // 1秒おきに状態を管理
    timer_ = this->create_wall_timer(1s, std::bind(&Supervisor::timerCallback, this));
  }

private:
  void timerCallback()
  {
    double elapsed = now().seconds() - start_time_;

    // 0秒でLowercaseNodeをActivate
    if (!hiragana_activated_ && elapsed >= 0.0) {
      RCLCPP_INFO(this->get_logger(), "Activating LowercaseNode...");
      configure_and_activate("lowercase_node");
      hiragana_activated_ = true;
    }

    // 20秒でLowercaseNodeをDeactivate → UppercaseNodeをActivate
    if (!katakana_activated_ && elapsed >= 20.0) {
      RCLCPP_INFO(this->get_logger(), "Switching to UppercaseNode...");
      deactivate("lowercase_node");
      configure_and_activate("uppercase_node");
      katakana_activated_ = true;
    }

    // 40秒で全LifecycleノードをShutdown
    if (!done_ && elapsed >= 40.0) {
      RCLCPP_INFO(this->get_logger(), "Shutting down all lifecycle nodes...");
      shutdown_lifecycle_node("lowercase_node");
      shutdown_lifecycle_node("uppercase_node");
      done_ = true;

      // Supervisor自体も終了
      rclcpp::shutdown();
    }
  }

  // Configure → Activate
  void configure_and_activate(const std::string & node_name)
  {
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
  }

  // Deactivate
  void deactivate(const std::string & node_name)
  {
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
  }

  // Deactivate → Cleanup → Shutdown
  void shutdown_lifecycle_node(const std::string & node_name)
  {
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE);
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP);
    change_state(node_name, lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
  }

  /**
   * ライフサイクルノードの /change_state サービスを呼んで状態を変更
   */
  bool change_state(const std::string & node_name, uint8_t transition_id)
{
  auto it = clients_.find(node_name);
  if (it == clients_.end()) {
    RCLCPP_ERROR(this->get_logger(), "No client for node: %s", node_name.c_str());
    return false;
  }
  auto client = it->second;

  // サービスが起動するまで待機 (これはOK。spin不要)
  if (!client->wait_for_service(std::chrono::seconds(2))) {
    RCLCPP_ERROR(this->get_logger(), "Service not available for node: %s", node_name.c_str());
    return false;
  }

  // リクエスト作成
  auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();
  request->transition.id = transition_id;

  // 非同期でサービス呼び出し
  auto future = client->async_send_request(
    request,
    [this, node_name](rclcpp::Client<lifecycle_msgs::srv::ChangeState>::SharedFuture response) {
      // ここは別スレッドで呼ばれるコールバック
      if (!response.get()->success) {
        RCLCPP_ERROR(this->get_logger(),
                     "Failed to change state for node: %s", node_name.c_str());
      } else {
        RCLCPP_INFO(this->get_logger(),
                    "Succeeded to change state for node: %s", node_name.c_str());
      }
    }
  );

  // ここでは同期待ちせず、即returnする
  // 成否はコールバック内でログを出すだけにする
  return true;
}




  std::vector<std::string> target_nodes_;
  std::unordered_map<std::string, rclcpp::Client<lifecycle_msgs::srv::ChangeState>::SharedPtr> clients_;
  rclcpp::TimerBase::SharedPtr timer_;
  double start_time_;

  bool hiragana_activated_{false};
  bool katakana_activated_{false};
  bool done_{false};
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto supervisor = std::make_shared<Supervisor>();
  rclcpp::spin(supervisor);
  rclcpp::shutdown();
  return 0;
}


```


</details>



### デモ
launchファイルで起動します
```
ros2 launch my_lifecycle_tutorial lifecycle_tutorial.py
```




動画

<iframe width="560" height="315" src="https://www.youtube.com/embed/t_dFOhb4vWo?si=yi04NFCl1PMtrSmb" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

中盤のこの部分で小文字→大文字になっているのが確認できました！

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3665619/45b4a081-2141-4e43-9d87-264e65ccc4d1.png)

<!---READMEははじめン本語でいいけど、後で英語で書き直す。日本語へのりんくを後で作ればOK--->


## 参考
1. [ROS2 Launchでライフサイクル制御](https://scrapbox.io/tygoto/ROS2_Launch%E3%81%A7%E3%83%A9%E3%82%A4%E3%83%95%E3%82%B5%E3%82%A4%E3%82%AF%E3%83%AB%E5%88%B6%E5%BE%A1)
1.  [Managed nodes](https://design.ros2.org/articles/node_lifecycle.html)
1.   [ROS 2ノードのライフサイクルを理解する (1) | Yutaka Kondo](https://www.youtalk.jp/2017/06/02/ros2-life-cycle.html)
1.    [ROS 2ノードのライフサイクルを理解する (2) | Yutaka Kondo](https://www.youtalk.jp/2017/06/03/ros2-life-cycle-demo.html)


# 謝辞
この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.

https://www.gxp.co.jp/

https://qiita.com/organizations/gxp


## Arcanain

https://github.com/Arcanain