環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
ROSのではエラーを扱う仕組みとしてDiagnosticという仕組みがあります。これの扱い方、集計の仕方、表示の仕方を説明します。
diagnostics
diagnosticsは主にハードウェアのエラーを取り扱うことを目的にした機構です。ハードウェアを直接扱うノードがハードウェアの状況を検知してそれを/diagnosticsというトピックに送ります。もちろんROSノードのソフトウェア的な状態の表示にも流用することが出来ます。
4つの状態
Diagnosticは各状態を4つの状態で管理をします。各ノードが自ら設定できるOK、WARN、ERRORの3つの状態と、トピックが不通になったSTALEがあります。
- OK : エラー無し
- WARN: 警告状態
- ERROR: エラー状態
- STALE: メッセージが届かない状態
各ノードの対応
diagnosticsは自動で出るものではなく以下のコードにあるように異常の基準をユーザーが決めて出すものです。また公開されているノードでも対応状況はまちまちです。例えばjoyコントローラーを扱うjoy_nodeでは対応していてコントローラーが接続されてないとエラーを出しますが、Webカメラを扱うuvc_camera_nodeでは対応していません。
diagnosticsの出力と表示
ソースコード
1秒ごとにOK、WARN、ERRORを切り替えて送信するROSノードのソースコードを以下に示します。
/diagnosticトピックをユーザーが直接publishするのではなくてdiagnostic_updater::Updaterというアクセサを介して扱います。
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <diagnostic_updater/diagnostic_updater.h>
int diagnostic_counter = 0;
void diagnostic0(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
if (diagnostic_counter % 30 < 10)
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "publish OK");
}
else if (diagnostic_counter % 30 < 20)
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN, "publish Warn");
}
else
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "publish Error");
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "info_diagnostic");
ros::NodeHandle nh;
// Diagnostic
diagnostic_updater::Updater updater;
updater.setHardwareID("DummyDevice");
updater.add("Dummy", diagnostic0);
ros::Rate loop_rate(10);
while (ros::ok())
{
updater.update();
ros::spinOnce();
loop_rate.sleep();
diagnostic_counter++;
}
return 0;
}
-
diagnostic_updater::Updater型でdiagnosticsを扱うインスタンスを生成します。 - updater.setHardwareID("DummyDevice")`でエラーを扱う対象の名前（ハードウェアに対応する名前）を設定します。この名前は後々の解析で使うこともできますがとりあえずは適当な名前で大丈夫です。
-
updater.add("Dummy", diagnostic0)でエラー状態を判断する関数を登録します。第1引数の名前は1つのノードで複数のdiagnosticを扱うときに区別するための名前です。適当な名前を付けて大丈夫です。 - 実際にエラー状態を判断するのは関数
void diagnostic0(diagnostic_updater::DiagnosticStatusWrapper &stat)です。この中で何らかの処理をしてstat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "publish OK")のようにノードのエラー状態を送信します。第1引数はOKかWARNかERRORかの区別、次の引数はコメントです（例えば同じERRORでも、トラブルの種類が違うものに違うコメントを付けることができる）。
これはサンプルプログラムなので適当な判定をしていますが、実用的にはsubscribeすべきデータが来ていなかったらWARNを出す。予想外の値だったらERRORを出すのような判断をします。
今回使うlaunchファイルを以下に書きます。上記のinfo_diagnosticsとjoy_nodeの2つを起動します。
<launch>
<node name="joy_node" pkg="joy" type="joy_node" />
<node name="info_diagnostic" pkg="info_lecture" type="info_diagnostic" />
</launch>
実行と表示
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch info_lecture diagnostics1.launch
rostopic echo /diagnosticsとするとROSトピックが流れているのが見えます。`
GUIで見る
ROSトピックでも見ることはできますが、ROSにはGUIでdiagnosticsを表示するツールがあります。rosrun rqt_runtime_monitor rqt_runtime_monitorで立ち上げることができます。
各状態別にdiagnosticsが分類されて表示されます。各表示を選択すると詳細な情報が見れます。
diagnosticsの集計
ハードウェごとにdiagnosticsが細々と出力されていると、ロボット全体としての状態はどうなのか集計した結果を見たくなります。そこで集計機能であるdiagnostic_aggregatorを使います。
今回は先ほど作った上記のinfo_diagnosticsとjoy_nodeの2つを起動し、それらの情報を集計します。
ソースコード
まずどのようにdiagnosticsを集計するかをyamlファイルで記述します。
type: AnalyzerGroup
path: robot
analyzers:
JoyDevice:
type: GenericAnalyzer
path: joy
startswith: joy_node
DummyDevice:
type: GenericAnalyzer
path: dummy
startswith: info_diagnostic
-
typeはanalizerで使うプラグイン名を書くところで通常は最上位はAnalyzerGroupで後はGenericAnalyzerを指定します。 -
pathは集計した結果として表示する時の名前空間を決めるものです。 - 残りのフィールドはどのdiagを集計するのかを特定します。以下のような特定の方法があります。複数を指定するとOR指定になります。
- name: 完全一致
- startswith: 前方一致
- contains: その文字列を含む
- regex: 正規表現
<launch>
<node name="joy_node" pkg="joy" type="joy_node" />
<node name="info_diagnostic" pkg="info_lecture" type="info_diagnostic" />
<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
<rosparam command="load" file="$(find info_lecture)/config/diagnostics.yaml" />
</node>
</launch>
実行と表示
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch info_lecture diagnostics2.launch
rostopic listをすると/diagnostics_aggというROSトピックが増えていることが見えます。
また集計した結果をGUIで表示するツールがROSにはあります。rosrun rqt_robot_monitor rqt_robot_monitorで起動します。
一番下のAll devicesのところがメインですべてのdiagnosticsがOKならば全体をまとめるrobotもOKになります。
Rviz上で表示
jsk_rviz_pluginを使うとdiagnosticsをRviz上で表示することができます。
ソースコード
<launch>
<arg name="rvizconfig" default="$(find info_lecture)/rviz/diagnostics.rviz" />
<node name="info_diagnostic" pkg="info_lecture" type="info_diagnostic" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
実行
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。
roslaunch info_lecture diagnostics3.launch
Rvizが立ち上がります。ros_lectureのgitからプログラムをとってきたのならrvizの設定ファイルを読み込んでいるはずですが、そうでないなら手動で設定が必要です。
- Rviz左下の「Add」を押します。
- メニューの中からOverlayDiagnosticsを選択します。
- トピックとdiagnosticsの名前の選択をします。
- トピック名は
/diagnosticsを選択します。 - 名前は今回の場合は
/vis_diagnostic:Dummyです。ノードの名前とupddater.addで指定した名前の組になります。ドロップダウンメニューですがなかなか選択肢に希望のものが入ってくれない場合があります。少し待っていると表示されるようになります。
- トピック名は
またこれ以外にもオプションがあります。
- typeは「SCA」と「EVA」があります。前者では
stat.summaryのコメントも表示してくれますが、後者のほうがコンパクトに表示されます。 - top、left、sizeは前回のjskのrvizプラグインと同じで表示位置と大きさの指定です。
これらを設定すると以下のように表示ができます。
ちなみに今回の表示では出てきていませんが、diagnosticを送信しているノードが消えて、送信が止まるとグレーになってSTALLと表示されます。
コメント
diagnosticsは凝り始めるときりがないので、できるだけシンプルに使うのが重要です。aggrigatorの分類でも迷うのですが、一番重要なことはミスを発見できるように設計することです。例えばPS3コントローラーをPCに接続をするのを忘れて起動することはよくあります。それがわかるようにjoy_nodeのdiagnosticsを画面に表示する。このように目的をもって分類しましょう。
参考
ROSにおける様々なエラーを管理するデフォルトツールdiagnostics入門
ROSwiki diagnostic_aggregator
Using the GenericAnalyzer