RasPiMouseのROS2ノードがTIER4のジェフさんによる実装が公開されています。
https://github.com/gbiggs/raspimouse2

このノードは、ROS2のLifecycleNodeを使って実装されており、今までのROSのノードと少し実装スタイルが異なっていると感じる人もいると思いますので、実装コードを見ながら解説していきます。

ROS2のノードやROS2での開発に関する日本語のドキュメントは、ジェフさんによるROS2の紹介記事があるようですので、ここでは詳細を省きたいと思います。ジェフさんのROS2の紹介に記事のサンプルでは、rclcpp::Nodeを使っているものから説明されていますので、ROS1のノードを開発したことがある人であればわかりやすいサンプルコードになっていると思います。

しかし、ROS2では、LifecycleNodeが導入されており、ジェフさんのRasPiMouseのノード実装は、こちらの実装スタイルになっているために若干わかりにくい人もいるかもしれません。

そもそも　rclcpp_lifecycle::LifecycleNode とは何なのでしょうか？

ROS1とROS2の違いの記事にもありますが、ROS2ではライフサイクルを持ったコンポーネントクラスを継承して、ノードを実装することが推奨されています。このライフサイクルを持ったコンポーネントのひな形となるのが、rclcpp_lifecycle::LifecycleNodeと思っても良いと思います。このライフサイクルを持ったコンポーネントは、内部に状態を持った状態マシンコンポーネントで、Unconfigured, Inactive, Active, Finalizeの4つの基本状態(Primary State)を持っています。

また、この4つの状態に対して7つの状態遷移処理 (''create'', ''configure'', ''cleanup'', ''activate'', ''deactivate'', ''shutdown'', ''destroy'')と6つの状態遷移中の状態(Transition State)（Configuring, CleaningUp, ShuttingDown, Activating, Deactivating, ErrorProcessing)が定義されています。

下図は、ROS2のLifecycleNodeの状態遷移図です。

詳細な解説は、ROS2 Designの解説を参照してもらった方が良いと思いますが、ここでも少し解説しておきます。

ROS2のノード（コンポーネントとも書いていますが）では、''create処理''でノードのコンストラクタが呼び出されインスタンスを生成し、Unconfigured状態になります。このUnconfigred状態は、初期化処理がインスタンス生成直後ですので、ノードの内部変数などは初期化されていない可能性もあります。通常C++の実装では、、rclcpp::init()からExecutorのspinメソッドが呼び出されるまでの処理にあたります。

Unconfigured状態からは、通常のプロセスでは、''configure処理''によってConfiguring状態になります。
Configuring状態の時に on_configureメソッド が呼び出され、成功すれば Inactive状態に遷移し、失敗すればUnconfigured状態に戻ります。
Inactivate状態は、ノードの実行前の状態にあたります。この状態では、ノードの内部状態が初期化されていることが期待できます。すなわち、on_configireメソッドでは、ノードの初期化やトピックやサービスの生成を行わなければいけないということです。

Inactive状態では、''active処理''によって Activating状態になります。
Activatng状態では、on_activateメソッドが呼び出され、成功すれば Active状態に遷移し、失敗すれば Inactive状態に遷移し、失敗すればUnconfigured状態に戻ります。Active状態は、ノードが動作している状態でROS2メッセージの送受信、サービスの実行等が動いている状態です。
ROS1でいうと whileループとか ros::spin()の実行状態です。

Inactive状態からは、''cleanup処理''でCleaningUp状態を経由してUnconfigured状態に戻ることもできます。
CleaningUp状態では、on_cleanupメソッドが呼び出され、成功すればUnconfigured状態へ遷移し、それ以外はエラーが発生します。

Acvive状態からは、終了するときは''shutdown処理''でShuttingDown状態に遷移します。
ShuttingDown状態では、on_shutdownメソッドが呼び出され、成功すればFinalizd状態に遷移します。
ここで失敗するというのは定義されていません。それは、通常はエラーが発生と同じ意味だからです。

Acvtive状態からは、''deactive処理''によって、Deactivating状態を経由してInactive状態に戻ることもできます。
Deactivasing状態では、on_deactivateメソッドが呼び出されて、成功すればInactive状態へ移行し、それ以外はエラーが発生します。

エラーの発生は、Configuring, CleaningUp, ShuttingDown, Activating, Deactivating, activateの7つの状態で発生します。
エラーが発生すると ErrorProcessing状態に遷移し、on_errorメソッドが呼び出されます。
on_errorメソッドが成功すればUnconfigred状態に遷移し、失敗すればFinalized状態に遷移します。

Finalizd状態は、ノードが終了した状態で ''destroy処理''でデストラクタが呼び出され、インスタンスが消滅します。通常、destroy処理が実行されるとExecutorのspin()がら抜けだしますので、 rclcpp::shutdown()でROS2を終了します。

以上を踏まえてRasPiMouseのROS2ノードの実装を見ていきましょう。

TIER4のジェフさんが公開されている実装コードを見ていきましょう。

このソースコードの関連ファイルは下記のようになっています。

• raspimouse : C++の実装コード
  • include/raspimouse/raspimouse_component.hpp: ROS2ノードのクラス定義（class Raspimouse)
  • src/rapimouse.cpp: ROS2ノードのmain関数
  • src/raspimouse_component.cpp: ROS2ノードのクラスの実装
• raspimouse_msgs: RasPiMouse用の独自メッセージ型の定義
  • msg/Leds.msg ：ロボット上のLED（4個）の制御用
  • msg/LightSensors.msg　：　ロボット上の赤外線距離センサの値（前後、左右）
  • msg/Switches.msg　：　ロボットのスイッチ用（3個）

raspimouse.cppには、main関数が定義されています。main関数は、C++の実行のメインになります。このコードを見てみると、

int main(int argc, char *argv[]) {
  /* ROS2の初期化処理　ノードの処理ではありません　*/
  rclcpp::init(argc, argv);

　/* 実行コンテキストであるExecutorの生成、この場合はSingleスレッドのExecutor */
  rclcpp::executors::SingleThreadedExecutor exe;　

　/* Raspimouseのインスタンス生成　*/
  std::shared_ptr<raspimouse::Raspimouse> raspimouse_node = 
  　　　　　std::make_shared<raspimouse::Raspimouse>();　

 　/* ノードのインスタンスを executorに登録 */
  exe.add_node(raspimouse_node->get_node_base_interface());
　/******* ここまでがノードの初期化です　*****/

　/* executorの実行ループを開始します。開始後は、Unconfigured状態です　*/
  exe.spin(); 

  /* ROS2を終了します　*/
  rclcpp::shutdown();
}

となっています。実装コードにコメントを付加していますので、何をしているかは大体わかると思います。

このコードで rclcpp::executors::SingleThreadedExecutor がありますが、このExecutorでROS2のノードは実行しています。ROS1のときは、1プロセス1ノードでしたが、ROS2になったときに1プロセスで複数のノードを実行できるようになりました。そのため、ノードを実行させるのにExecutorという概念が導入されたということです。

詳細な説明は、ジェフさんの「ROS 2のAPIの使い方」を参照してください。

次に、raspimouseコンポーネントの実装コードの詳細を見ていきましょう。実装コードは、raspimouse_component.cppになります。

14行目まではコメントになっており、15行目～26行目までは、ヘッダーファイルのインクルード、28行目もnemespeceの使用宣言なので問題ないと思います。

30行目～36行目は、グローバル変数（定数）の定義です。''constexpr auto''　で宣言されています。''constexpr''はC++11で導入された記法です。汎用的に定数式を表現するための機能です。

38行目以降に raspimouse::Rsapimousのクラスで定義されているメソッドの実装が記載されています。定義されている関数は以下の通りです。

Raspimouse()

41行目～53行目、コンストラクタ

on_configure(const rclcpp_lifecycle::State &)

55行目～202行目、Configuring状態で呼び出されるメソッド

on_activate(const rclcpp_lifecycle::State &)

204行目～222行目、Activating状態で呼び出されるメソッド

on_deactivate(const rclcpp_lifecycle::State &)

224行目～242行目、Deactivating状態で呼び出されるメソッド

on_cleanup(const rclcpp_lifecycle::State &)

245行目～273行目、CleaningUp状態で呼び出されるメソッド

publish_odometry()

275行目～310行目、オドメトリ情報（メッセージ）を発行する

publish_switches()

312行目～353行目、3つのスイッチのデバイスをオープンして、その状態を読み込んでメッセージを発行する

pulish_light_sensors()

355行目～369行目、赤外線距離センサのデバイスをオープンして、センサ情報を読み込みメッセージを発行する

velocity_command(const geometry_msgs::msg::Twist::SharedPtr msg)

371行目～383行目、Twist型のメッセージを引数で受け取り、それに応じた速度を左右のモータの指令値を設定する

leds_command(const raspimouse_msgs::msg::Leds::SharedPtr msg)

385行目～410行目、Leds型のメッセージを引数として、各LEDを点けたり消したりします

buzzer_command(const std_msgs::msg::Int16::SharedPtr msg)

412行目～415行目、引数の値に応じてブザーを鳴らします。

handle_motor_power(const std::shared_ptr<rmw_request_id_t> request_header,const std::shared_ptr<std_srvs::srv::SetBool::Request> request,const std::shared_ptr<std_srvs::srv::SetBool::Response> response)

417行目～430行目、モータのON/OFを行うサービスの定義です

watchdog()

432行目～438行目、モータを止めて, watchdog_timer_をリセットします

set_motor_power(bool value)

440行目～455行目、valueが真であれば、モータのパワーをONにしwatchdog_timer_をリセットし、偽であればモータ止めてwatchdog_timer_をキャンセルします

stop_motors()

457行目～461行目、モータを停止します

calculate_odometry_from_pulse_counts(double &x, double &y, double &theta)

463行目～511行目、パルスカウントからオドメトリを計算し、参照引数に位置と回転角を入れます

estimate_odometry(double &x, double &y, double &theta)

513行目～522行目、オドメトリを推定し、参照引数に位置と回転角を入れます

526行目には、class_loader/registor_macro.hppをインクルードして、CLASS_LOADER_REGISTER_CLASS() でRaspimouseクラスを登録しています。