ロボットを制御するときに、一定周期で実行したくなります。ただし、汎用OS上で動かす場合、他のプロセスの優先順位などの関係から変動(ジッタ)が発生します。
PREENPT_RTというリアルタイム性を向上させるパッチを適用したLinuxカーネルを用いて、一定周期で実行するC言語プログラムを紹介します。
ハードウェア準備
用意するものはRaspberry Pi 4Bと32GBのmicorSDカードです。モニタやキーボード、ネットワークケーブルは必要に応じて接続します。
SBC：Raspberry Pi 4 B
microSD：32GB
ディストリビューションのダウンロード
PREENPT_RTのパッチをLinuxカーネルにあてる詳細な方法は、ネット等で検索すれば、有志によるサイトを見つけることができます。
今回はお手軽に、GitHubに公開されているPREEMPT_RT適用済みのUbuntuをダウンロードしてRasPiで使ってみます。ROS 2 Humbleも入っています。
以下のURLにアクセスします。
私が試したのは、ROS 2 Humble (22.04.3, 5.15.98-rt62-raspi)になります。
OS：Ubuntu 22.04.3
RTカーネル：PREEMPT_RT
ミドルウェア：ROS 2 Humble
書き込み
Raspberry Pi Imagerで、ダウンロードしたimgファイルをmicroSDに書き込みます。
OSを選択するときに、Use Customを選んで、ダウンロードしたimgファイル(圧縮ファイルのままでOK)”ubuntu-22.04.3-rt-ros2-arm64+raspi.img.xz”を指定します。
確認
現在実行中のカーネルについての名前と情報を確認します。PREEMPT_RTと書かれているので、PREEMPT_RTが適応されているのがわかります。
$ uname -a Linux ubuntu 5.15.98-rt62-raspi #1 SMP PREEMPT_RT Sun May 7 10:39:42 UTC 2023 aarch64 aarch64 aarch64 GNU/Linux
現在実行中のカーネルが、実際にリアルタイム対応をしているかどうかを、以下のコマンドでも確認ができます。
$ cat /sys/kernel/realtime 1
ROSのバージョンは何かを確認します。
$ echo $ROS_DISTRO humble
一定周期プログラム
pthreadを使って、一定周期で実行する関数のマルチスレッドプログラムになります。
$ vi interrupt_pthread.c
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <unistd.h>
#include <time.h>
#define INTERRUPT_INTERVAL_NS 10000000 // 10ミリ秒
void interrupt_main(void) {
// 割り込み処理
printf("Interrupt occurred\n");
}
void* interrupt_handler(void* arg) {
while (1) {
struct timespec start, end, ts;
long elapsed_time_ns;
// 開始時間の記録
clock_gettime(CLOCK_MONOTONIC, &start);
// 割り込み処理
interrupt_main();
// 終了時間の記録
clock_gettime(CLOCK_MONOTONIC, &end);
// 経過時間の計算（ナノ秒）
elapsed_time_ns = (end.tv_sec - start.tv_sec) * 1000000000L + (end.tv_nsec - start.tv_nsec);
// 待機時間の計算（INTERRUPT_INTERVAL_NSから経過時間を引く）
ts.tv_sec = 0;
ts.tv_nsec = INTERRUPT_INTERVAL_NS - elapsed_time_ns; // 10ミリ秒 - 経過時間
// 待機時間が負の場合は0にする
if (ts.tv_nsec < 0) {
ts.tv_nsec = 0;
}
// 待機
nanosleep(&ts, NULL);
}
return NULL;
}
int main() {
pthread_t thread;
// 割り込み処理スレッドの作成
if (pthread_create(&thread, NULL, interrupt_handler, NULL) != 0) {
fprintf(stderr, "Error creating thread\n");
return 1;
}
// メインスレッドは他の処理を行う
while (1) {
// 他の処理
sleep(1);
}
// スレッドの終了を待機
pthread_join(thread, NULL);
return 0;
}
$ gcc -o interrupt_pthread interrupt_pthread.c $ ./interrupt_pthread
ちなみに、上記のプログラムはVScodeのCopilotに「pthreadを使って10ミリ秒毎に割り込み処理するC言語プログラムを書いて」と指示して、出力されたコードにさらに指示を加えたものです。
「一定周期で割り込み処理するプログラムの処理時間を計測して、処理時間が変動しても10ミリ秒間隔が変わらないように」など。
マイコンなどのタイマー割り込みと違い、一定時間になるようnanosleep関数で待つという処理を行っています。