🍓

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

2025/02/02に公開

Raspberry Pi

前回ROS2のセットアップをしたので、今回はマイコン側のビルド環境のセットアップとmicro-ROSの導入について記載します。

まず今回は、Pico上でmicro-ROSを使ってPicoからROS2側にメッセージを投げて拾ってみるところをやります。

サンプルプログラムのリポジトリ

環境

ホストはRaspberry Pi Zero 2 (Ubuntu22.04 + ROS2 Humble)
エッジはRaspberry Pi Pico、Pico SDKを使ってビルド
Picoモジュールのビルドは VSCode + 拡張機能でクロスコンパイル
micro-ROSのライブラリ Github micro-ROS/micro_ros_raspberrypi_pico_sdk

ビルドまでのセットアップ

1. PicoProbeを作成する

まず、もう一つのPicoをProbe化してSWD経由で書き込みできるようにする。デバッグもできてこれがベストだと思います。

2. Raspberry Pi PicoのVSCode拡張機能をインストールする。

Pico SDKやクロスコンパイルするためのコンパイラやリンカ、デバッガなど一通りセットアップしてくれるので必須だと思います。手作業でもでも試したけど、結局こちら使ってます。USBケーブルでPicoと開発PCを接続して作業できるようにします。VSCodeのシリアルモニタでも確認できるようになります。

まずはPicoProbe経由でPicoにblinkなどのサンプルをビルド＆FlashしてLチカできるようになればOK。

3. micro-ROSのリポジトリからモジュールソース、ライブラリソースなど入手

Raspberry Pi PicoのVSCode拡張機能をインストールすると、Windowsの場合だとユーザフォルダ直下にPico SDKがインストールされる。

blinkなどサンプルプロジェクト直下の.vscode/settings.jsonに環境変数PICO_SDK_PATHの記載がある。

"PICO_SDK_PATH": "${env:USERPROFILE}/.pico-sdk/sdk/2.1.0",

とりあえず、micro-ROSも上記と同じ場所にクローンしておきます。ROS2のディストリビューションごとでブランチが分かれているので今回はhumbleを使う。

cd %USERPROFILE%
git clone --recurse-submodules https://github.com/micro-ROS/micro_ros_raspberrypi_pico_sdk.git -b humble

上記のリポジトリには、Pico向けのROS2と通信するためのプログラムやビルド済みのライブラリなどが格納されている。また自分のビルド環境でライブラリをビルドすることもできる。

今回はリポジトリに登録されているビルド済みのライブラリをコピーしてそのまま使うことにします。

4. サンプルプログラムのプロジェクトを作成する

VSCode拡張機能からプロジェクトを作成します。

micro-ros-sampleとしてます。ボードはPico使ってます。

基本デフォルトでOKですが、とりあえずConsole over UARTはONしました。

ワークスペースが作成できたら、さきほどクローンしたmicro-ROSのリポジトリの中から以下をコピーしてもってくる。

libmicrorosフォルダ
pico_uart_transport.c
pico_uart_transports.h

ワークスペースは以下のようになります。

次にCMakeLists.txtを編集して、micro-ROSのライブラリなど追加してビルドできるようにします。

ライブラリのディレクトリ、ソースを追加します。

# micro-ros library
link_directories(libmicroros)

# Add executable. Default name is the project name, version 0.1

add_executable(micro-ros-sample
    micro-ros-sample.c
    pico_uart_transport.c
)

リンクするライブラリ、ヘッダ、コンパルオプションを追加しました。いずれもリポジトリのサンプル内にあったcmakeを真似ているだけです。

# Add the standard library to the build
target_link_libraries(micro-ros-sample
        pico_stdlib
        microros
)

# Add the standard include files to the build
target_include_directories(micro-ros-sample PRIVATE
        ${CMAKE_CURRENT_LIST_DIR}
        libmicroros/include
)

SET(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -ffunction-sections -fdata-sections")
SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -ffunction-sections -fdata-sections")
add_compile_definitions(PICO_UART_ENABLE_CRLF_SUPPORT=0)
add_compile_definitions(PICO_STDIO_ENABLE_CRLF_SUPPORT=0)
add_compile_definitions(PICO_STDIO_DEFAULT_CRLF=0)

5. プログラムの実装

micro-ros-sampleは以下にあります。

micro-rosを使ってホスト（ROS2）と疎通確認する
"pico_node"ノードを作成する
"pico_publish_topic"トピックを作成する
タイマーを定義して1sec毎にコールバックする
タイマーコールバック内でPicoボードのLED（GPIO25）をLチカさせる

プロラムはGithubにあります。

ROS2とデータを送受信するためのカスタム関数を登録します。関数はpico_uart_transport.cに定義されている。（後述）

    rmw_uros_set_custom_transport(
        true,
        NULL,
        pico_serial_transport_open,
        pico_serial_transport_close,
        pico_serial_transport_write,
        pico_serial_transport_read
    );

開始前に疎通確認する。ROS2とただしくシリアル接続できてないと、ここでエラーになる。

ROS2側はmicro-ROS Agentが起動していること
ROS2側、PICO側はUART接続できていること

    // micro-ROSエージェントへの接続が確立できるか確認します。
    // 接続できなければ、プログラムは終了します。(15sec)
    const int timeout_ms = 1000; 
    const uint8_t attempts = 15;
    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
    if (ret != RCL_RET_OK)
    {
        // Unreachable agent, exiting program.
        printf("ERROR) Unreachable agent, exiting program.\n");
        return ret;
    }

ノードやトピックを作成する。メッセージの型はInt32で整数をやり取りします。

    // デフォルトのメモリアロケータを取得します。
    rcl_allocator_t allocator;
    allocator = rcl_get_default_allocator();
    // ROS 2システム全体で使用するサポートオブジェクトを初期化します。
    // これは、後続のノードやタイマーの初期化に利用されます。
    rclc_support_t support;
    rclc_support_init(&support, 0, NULL, &allocator);
    
    // "pico_node" という名前のノードを作成します。
    // ノードはROS 2における基本の実行単位です。
    rcl_node_t node;
    rclc_node_init_default(&node, "pico_node", "", &support);
    
    // std_msgs/msg/Int32 型のメッセージを 
    // "pico_publish_topic" というトピックで発行するパブリッシャーを初期化します。
    rclc_publisher_init_default(
        &publisher,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
        "pico_publish_topic");

タイマーを定義してコールバックを指定します。

    // 1秒（1000ms）ごとに実行されるタイマーを初期化し、
    // そのタイマーのコールバック関数として timer_callback を登録します。
    rcl_timer_t timer;
    rclc_timer_init_default(
        &timer,
        &support,
        RCL_MS_TO_NS(1000),
        timer_callback);
    
    // エグゼキューター（実行ループ管理オブジェクト）を初期化し、
    // 先ほど作成したタイマーを rclc_executor_add_timer() で登録します。
    // エグゼキューターは、登録されたタイマーやサブスクライバーの
    // コールバックを管理し、定期的に実行します。
    rclc_executor_t executor;
    rclc_executor_init(&executor, &support.context, 1, &allocator);
    rclc_executor_add_timer(&executor, &timer);

最後にwhileループでブロックします。ループ内で、rclc_executor_spin_some()をコールし続けます。全体の流れは以上になります。

    while (true) {
        // 登録されたコールバック（タイマー、サブスクライバー、サービスなど）を
        //「順次」実行するための関数です。つまり、複数のコールバックがエグゼキューターに
        // 登録されていた場合、同時並行（並列）に実行されるのではなく、一つずつシーケンシャルに実行されます。
        // 100ミリ秒をナノ秒に変換した値を指定しており、呼び出しでコールバックを
        // チェックまたは実行する際のタイムアウトの上限時間を指定する

        // エグゼキューターは、100ミリ秒分の時間（ナノ秒単位）だけ待機し、
        // その間に到着した（または実行可能になった）コールバックを順次処理する。
        // もし100ミリ秒の間に実行すべきコールバックが存在しない場合、
        // タイムアウトとなり、処理を抜ける。

        // while(true)ループ内で何度も実行する必要がある（都度コールバックするため）
        // タイムアウト値が設定されているので、「最大100ms」ウェイトされる。
        // 調整の為、スリープを入れる必要がない。
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
        
    }

1秒ごとに以下のコールバック関数が実行されます。メッセージ値（整数）をインクリメントしてパブリッシュしてます。偶数の時にLEDをON、奇数のときにOFFさせることで点滅します。

// timerコールバック
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
    // 指定されたパブリッシャーを通して、メッセージ（ここでは std_msgs__msg__Int32 型）を発行します
    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
    // 送信後に msg.data をインクリメント
    msg.data++;
    printf("timer callback msg.data = %d\n", msg.data);
    gpio_put(LED_PIN, msg.data % 2);
}

転送プログラムについて、サンプルではデフォルトのUART0を使っている実装ですが、私の場合は

標準出力などのデバッグメッセージはUART0
ROS2、micro-ROSのやり取りはUART1

と分けたかったので、以下のように修正してます。


#include "hardware/uart.h"
#include "hardware/gpio.h"

/*
micro-ROS <=> ROS2のカスタムデータ転送を実装する
サンプルを元にしているが、デフォルトだとUART0を使っていて、
DebugProbeで利用しているデバッグ出力とバッティングする
そこで、こちらのシリアル通信をUART1（GPIO4、GPIO5）に変更する
*/
#define UART_ID       uart1      // 使用するUART
#define BAUD_RATE     115200     // ボーレート（必要に応じて調整）
#define UART_TX_PIN   4          // UART1のTXに使うGPIOピン（例: 4番ピン）
#define UART_RX_PIN   5          // UART1のRXに使うGPIOピン（例: 5番ピン）
/*
以下をワイヤ接続する
micro-ROS TX(GPIO4) <==> ROS2 RX
micro-ROS RX(GPIO5) <==> ROS2 TX
micro-ROS GND       <==> ROS2 GND ※GNDも忘れずに接続
*/

// UART1の初期化例
void init_uart1(void)
{
    uart_init(UART_ID, BAUD_RATE);
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);
}

bool pico_serial_transport_open(struct uxrCustomTransport * transport)
{
    // UART1 初期化
    init_uart1();
    return true;
}
size_t pico_serial_transport_write(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, uint8_t *errcode)
{
    uart_write_blocking(UART_ID, buf, len);
    return len;
}
size_t pico_serial_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
{
    size_t count = 0;
    uint64_t start_time_us = time_us_64();
    while (count < len)
    {
        if(uart_is_readable(UART_ID))
        {
            buf[count] = uart_getc(UART_ID);
            count++;
        }
        else if (time_us_64() - start_time_us > timeout * 1000)
        {
            *errcode = 1;
            break;
        }
    }
    return count;
}

6. プログラムのコンパイル

とりあえず、プログラムが自身の環境でビルドできるか確認します。Compile Projectをクリックします。

[64/64] Linking CXX executable micro-ros-sample.elf

エラーが発生せずに、下図のようにモジュールが出来ていれば成功。

失敗する場合は以下を確認。

Pico SDKがインストールされている（拡張機能がやってくれる）
libmicrorosフォルダごとコピーして配置している
cmakeがにmicorrosライブラリが追加されている

長い。。。一旦投稿を区切ります。その２に続きます。