ページ

▼

2023年1月30日月曜日

platformioでmicro rosのpub sub reconnection


背景

micro rosとはros2でros1のrosserialのようにマイコンからtopicの配信(publish)や購読(subscribe)ができるライブラリです。
以前試したときはシリアルポート接続の2回目以後の接続確立に手動でマイコンをリセットするしか方法が無いと思っていましたが、再接続の例があったのでそれを組み込んだらrosserialのようにシリアルポートを開く度に接続確立が可能になりました。

備忘録として記述方法とmicrorosの動作方法を記事に残します。

使ったもの

  • ubuntu22.04をインストールしたPC
  • ros2 rolling
    インストール手順はこちらです
    Installation Ubuntu (Debian) - Ros2 Rolling
  • platformio
    vscodeと合わせて利用しました。
    vscodeと連携させる手順はこちらです。
    PlatformIO IDE for VSCode
  • ESP32開発ボード + USBケーブル
    esp32devkitcやM5Stackで動作確認できます
  • docker
    シリアルポートの接続でdockerのコンテナを利用します
    インストール手順はこちらです
    Install Docker Engine on Ubuntu

platformio.iniの設定

micro ros arduinoとmicro ros platformioがあり、前者の方が開発が活発で情報が多いのでお勧め

microrosをplatformioで開発するには、micro ros arduinoとmicro ros platformioのどちらかが使えます。
どちらもmicro rosが公式に開発しているライブラリです。

自分が思うそれぞれの利点はこちらです

micro ros arduinoの利点
  • サンプルプログラムが多い
  • 対応表に載ってないマイコンの対応試みが行われている（stm32シリーズのイーサネットなど）

ｍicro ros platformioの利点
  • ビルド処理がライブラリに含まれるので、rosのtopicなどの実装変更を取り込みやすい
  • 最新のコードで対象のディストリビューションを使える

arduinoの方で開発されたマイコンの処理をplatformioに適用する流れだと思うので、どちらでも良いなら開発が先に進むmicro ros arduinoの方を使うのをお勧めします。

micro ros arduinoを使う場合

ライブラリとしてmicro ros arduinoをgithubのurlを指定しつつ、ビルド済みのライブラリ名libmicrorosをbuild_flagsで指定します。

platformio.ini
[env:esp32dev-micro-ros-arduino]
platform = espressif32
board = esp32dev
framework = arduino
lib_deps = 
    https://github.com/micro-ROS/micro_ros_arduino.git
build_flags =
    -l libmicroros

ビルド済みライブラリのパスの指定が必要と紹介している記事を見かけましたが、自分の環境では不要でした。
build_flags =
    -L ./.pio/libdeps/esp32dev-micro-ros-arduino/micro_ros_arduino/src/esp32/

micro ros platformioを使う場合

ライブラリとしてmicro ros platformioのgithubのurlを指定しつつ、board_microros_distroでros2のディストリビューション（バージョン）を指定します。

platformio.ini
[env:esp32dev-micro-ros-platformio]
platform = espressif32
board = esp32dev
framework = arduino
lib_deps =
    https://github.com/micro-ROS/micro_ros_platformio
board_microros_distro = rolling

micro ros platformioはライブラリをビルドするので、自分の環境だと初回ビルドは10分弱かかりました。
2回目以後はビルド済みのファイルを利用するためmicro ros arduinoと同様のビルド時間になります。

libmicrorosというフォルダに指定したディストリビューションの関連ファイルが自動で配置されます。
何が配置されるかはmicro_ros_platformio/microros_utils/repositories.pyを見ると分かります。

自分が把握したmicro ros arduinoと異なるプログラム内の記述は下記2点です。
  • ライブラリのヘッダファイルがmicro_ros_platformio.hになる
    // #include <micro_ros_platformio.h>
    #include <micro_ros_arduino.h>
  • set_microros_transportsではなくset_microros_serial_transportsでシリアルポートを設定する
    // set_microros_transports();
    Serial.begin(115200);
    set_microros_serial_transports(Serial);

pub sub reconnectionプログラム

全体像: githubに上げました

コードは200行近くあるのでここには貼り付けずgithubに上げました。

practice-pub-sub-reconnect-micro-ros/src/main.cpp

下記の動作をするプログラムです。
  • USB接続が途切れても再接続時にrosの接続も再開する
  • /micro_ros_arduino_node_publisherで数字を1加算しながら1秒ごとに配信
  • /micro_ros_arduino_subscriberで受け取った数字でpublisherの数字を更新

参考にしたプログラム

micro ros arduinoの下記のpublish、subscribe、reconnectionプログラムを参考にしつつ統合しました。
micro_ros_arduino/examples/micro-ros_publisher/micro-ros_publisher.ino
micro_ros_arduino/examples/micro-ros_subscriber/micro-ros_subscriber.ino
micro_ros_arduino/examples/micro-ros_reconnection_example/micro-ros_reconnection_example.ino

詰まったこと

executor_initで指定するhandle数はexecutorにaddする仕事に比例して増やす必要がある

下記のコードはpublishとsubscribeのサンプルコードを統合したことでexecutorにtimerとsubscriberを登録しています。
それに伴いexecutor_initに渡す数を1から2に増やす必要がありました。
増やしていないと2番目のaddでRCCHECKが異常を検知して無限ループに入り動作を停止します。
  executor = rclc_executor_get_zero_initialized_executor();
  RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));
  RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msgSub,
                                         &subscription_callback, ON_NEW_DATA));

ROSIDL_GET_MSG_TYPE_SUPPORTに渡すのはmsgのままで良い

publihとsubscribeを統合する際にmsgの変数名に用途を付与しました。
しかしながら、ROSIDL_GET_MSG_TYPE_SUPPORTは、変数が必要なのではなく、内部で受け取った文字列に応じて文字列を生成するマクロなので、msgを渡すままで良かったです。
std_msgs__msg__Int32 msgSub;
std_msgs__msg__Int32 msgPub;

bool create_entities() {
  // create publisher
  RCCHECK(rclc_publisher_init_default(
      &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
      "micro_ros_arduino_node_publisher"));
  // msgという変数も型も定義されていませんが、文字列生成マクロなのでmsgのままで良いです
}

動作確認

プログラムを書き込んだESP32をUSB接続したままにして、ros2が読み込まれたターミナルで下記のコマンドを実行してmicro rosのシリアルポートを開きます。
docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO serial --dev /dev/ttyUSB0 --baud 115200

認識されると下記のようなログが表示されます。
rosserialとの違いとして、ログを見ただけでは何のtopicが作られたか分かりません。
[1675002724.988128] info     | TermiosAgentLinux.cpp | init                     | running...             | fd: 3
[1675002724.988553] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1675002725.400145] info     | Root.cpp           | create_client            | create                 | client_key: 0x7A968064, session_id: 0x81
[1675002725.400253] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x7A968064, address: 0
[1675002725.425759] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x7A968064, participant_id: 0x000(1)
[1675002725.441489] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x7A968064, topic_id: 0x000(2), participant_id: 0x000(1)
[1675002725.450644] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0x7A968064, publisher_id: 0x000(3), participant_id: 0x000(1)
[1675002725.461245] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0x7A968064, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1675002725.476517] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x7A968064, topic_id: 0x001(2), participant_id: 0x000(1)
[1675002725.485760] info     | ProxyClient.cpp    | create_subscriber        | subscriber created     | client_key: 0x7A968064, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1675002725.496587] info     | ProxyClient.cpp    | create_datareader        | datareader created     | client_key: 0x7A968064, datareader_id: 0x000(6), subscriber_id: 0x000(4)

再接続処理を有効にしているので、先程のコマンドをCtrl - cで止めて再度実行すると再び接続されます。（再接続処理が無いと、2回目以後はマイコンのリセットが必要になります。）

他のターミナルでtopicを確認します。
ros2 topic list

期待通り/micro_ros_arduino_node_publisherと/micro_ros_arduino_subscriberが表示されました。


topicを受け取ってみます。
ros2 topic echo /micro_ros_arduino_node_publisher

毎秒1ずつ増える数字を確認できました。


別のターミナルでsubscriberのtopicに数字を配信してみます。
ros2 topic pub --once /micro_ros_arduino_subscriber std_msgs/Int32 "{data: 5}"

publisherで確認している数字が5に変わりました。


publisherもsubscriberも期待通りに動いていると分かりました。

終わり

micro rosをESP32で利用して、シリアルポート再接続時の処理を有効にした状態でpublisherとsubscriberを動かせました。
以前利用したときは再接続ができなくて運用に何があると判断しましたが、再接続が可能になったことで既に動いているマイコンとPCの接続が問題なくできるためrosserialの置き換えを試みれます。

参考

今回利用したライブラリです。
micro ros arduino

micro rosの挙動の全体像把握の参考にした記事です。
microROS-ArduinoのM5Atom用設定（ESP32・ROS2）

今回は問題になりませんでしたが、platformioでビルドできなかった場合の対応が紹介されているgithubのissueページです。
Compilation fails for any example project for ESP32 on Platformio

再接続処理の存在を知ったスレッドです。
micro-ros agent only starts for the second time after uC manual reset

ターミナルで実行するpublishとsubscribeのコマンド組み立ての参考にした記事です。
First micro-ROS Application on Linux
【micro-ROS for Arduino】簡単なPublisherとSubscriberの書き方

at

0 件のコメント :

コメントを投稿