🍓

ラズパイPicoでmicro-ROSを使う(その３)

2025/02/05に公開

Raspberry Pi

前回の続き

前回はパブリッシャーを実装しましたが、今回はサブスクライバを実装して、ROS2側からトピックにメッセージをパブリッシュします。Pico側でそのメッセージをサブスクライブできるか確認します。

Picoでサブスクライバの実装

micro-ros-sample2にUPしました。micro-ros-sampleを流用してサブスクラバを追加してます。今回はポイントだけです。

エグゼキュータの初期化ですが、今回はタイマーだけでなく、サブスクライバも登録が必要になります。第3引数で管理するハンドル数を指定するので、今回は2個になります。1のままだと、2個目に登録したハンドルが機能しないです。

// エグゼキュータの初期化
// rclc_executor_add_subscription
// rclc_executor_add_timer
// 2個のハンドルを管理する
int num_of_handles = 2;
rclc_executor_t executor;
rclc_executor_init(&executor, &support.context, num_of_handles, &allocator);

どんなメッセージを受信するか型とトピック名を定義して、エグゼキュータにサブスクリプションを登録します。コールバックはtopic1_subscribeという関数です。

// サブスクリプションの作成
// トピック名: pico_topic1
// メッセージ型: std_msgs/msg/Float32
rcl_subscription_t subscriber;
rclc_subscription_init_default(
    &subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "pico_topic1");

// エグゼキュータにサブスクリプションを登録
std_msgs__msg__Float32 msg;
rclc_executor_add_subscription(
    &executor, &subscriber,&msg, topic1_subscribe, ON_NEW_DATA);

topic1_subscribeを実装します。取得したメッセージをFloat型に変換してprintf()で出力しているだけです。

// サブスクリプションのコールバック関数
void topic1_subscribe(const void * msgin)
{
  const std_msgs__msg__Float32 * msg_float = (const std_msgs__msg__Float32 *)msgin;
  if (msg_float == NULL) {
    return;
  }
  // 受信した float データを表示
  printf("[subscribe topic1] %.2f\n", msg_float->data);
}

サブスクライバの追加は以上です。タイマーの追加と大して変わらないです。

実行してみる

前提として、ROS2側を今回はRaspberry Pi Zero 2でPicoとUART接続が済んでいるところから説明。できてない場合は、過去の投稿みてください。

Zero2にsshログインして以下を実行してmicro-ROS Agentを起動します。

> source install/local_setup.sh
> ros2 run micro_ros_agent micro_ros_agent serial -b 115200 --dev /dev/ttyAMA0

micro-ros-sample2をビルドしてPicoにフラッシュします。
VSCodeのシリアルモニタで確認すると、パブリッシュはすぐに開始します。

-------------------------
micro-ros-sample2 start
OK1) Ping has reached the micro-ROS agent.
OK2) rcl_get_default_allocator.
OK3) rclc_support_init.
OK4) rclc_node_init_default.
OK5) rclc_executor_init.
OK6) rclc_subscription_init_default.
OK7) rclc_executor_add_subscription.
OK8) rclc_publisher_init_default.
OK9) rclc_timer_init_default.
OK10) rclc_executor_add_timer.
[publish topic2] 0
[publish topic2] 1
[publish topic2] 2
：

Zero2側でターミナルを追加して、pico_topic1トピックに1回だけパブリッシュしてみます。

> source install/local_setup.sh
> ros2 topic pub /pico_topic1 std_msgs/msg/Float32 "{data: 12.3}" --once

Pico側のシリアルモニタを確認すると、定期的にパブリッシュしている間にサブスクライブしたメッセージが表示されます。

[publish topic2] 10
[publish topic2] 11
[publish topic2] 12
[subscribe topic1] 12.30
[publish topic2] 13
[publish topic2] 14
[publish topic2] 15

--oneceオプションを外すと連続でメッセージを投げるので、遅延なく拾えてることも確認できます。

[publish topic2] 93
[subscribe topic1] 12.30
[publish topic2] 94
[subscribe topic1] 12.30
[publish topic2] 95
[subscribe topic1] 12.30
[publish topic2] 96
[subscribe topic1] 12.30
[publish topic2] 97
[subscribe topic1] 12.30
[publish topic2] 98
[subscribe topic1] 12.30

注意点

ROS2側とはシリアルケーブルで繋がってます。エージェントが落ちたり、ROS2側が再起動すると、以降のやり取りができなくなる感じで、ROS2側を再起動したときなどは、エージェントが起動したのを確認してから各種処理しないとダメです。

処理の最初にrmw_uros_ping_agent()で疎通確認してます。疎通できてから処理が進むようにしたり、断線したりして通信できなくなったら再起動させる工夫が必要だと思いました。

なので、Picoの給電は、ROS2側の本体が起動してエージェントが起動したら、給電できるような仕組みが良いと思います。

以上

Picoでサブスクライバの実装

実行してみる

注意点

Discussion