MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

ROSにおける様々なエラーを管理するデフォルトツールdiagnostics入門

目次

 

はじめに

この記事は2015年ロボットアドベントカレンダーの22日目の記事です。

qiita.com

 

ROSの様々なエラーを管理するデフォルトツールdiagnostics

ロボットにとって、

ハードウェアやソフトウェアのエラーを

瞬時にハンドリングすることは必須だと思います。

 

そこで便利なのが、

ROSのデフォルトツールであるdiagnosticsです。

 

今回は、このdiagnositcsの

基本的な使い方について説明したいと思います。

 

diagnosticsの概要

diagnositicsは、

ハードウェアなどからの情報から、

ユーザがエラーデータを

ロギングしたり、解析したりしやすくするための

ツールチェーンです。

 

エラーデータのデータフォーマット(diagnostic_msgs)や

エラーデータの送受信ツール(diagnostic_updater)、

リアルタイムにエラーデータをカテゴリー分けや解析するツール

(diagnostic_aggregator)

エラーデータを可視化するツール

(rqt_robot_monitor, rqt_runtime_monitor, OverlayDiagnostic)、

そして、そのデータを後処理で解析するツール(diagnostic_analysis)

などで構成されています。

 

続いてこれらのツールについて概要を説明します。

 

エラーデータのデータフォーマット: diagnostic_msgs

diagnosticのエラー情報は、

diagnostic_msgsのトピックとして送信します。

 

基本は下記のリンク先の

diagnostic_msgs/DiagnosticStatusにエラーデータを入れ、

DiagnosticArrayに複数のエラー情報を格納して送信する形になります。

 

基本的な設計思想としては、

DiagnosticStatusにはハードウェアidを与えることができるため、

一つのハードウェアに対して、

一つのDiagnosticStatusを割り当て、

そのハードウェアのエラーメッセージやエラーID、

もしくはエラー判定条件とその値を

KeyValueとして格納して送信する形が一般的なようです。

 

diagnosticのメッセージのステータス

diagnosticのメッセージのステータスは

下記の4つで表現されます。

  1. OK : エラー無し

  2. WARN: 警告状態

  3. ERROR: エラー状態

  4. STALE: メッセージが届かない状態

 

エラーデータの送受信ツール: diagnostic_updater

diagnosicのメッセージトピックを送信する場合は、

diagnositic_updaterというツールを使います。

c++とpythonのAPIが提供されており、

それぞれほぼ同じAPIで利用することができます。

 

オフィシャルなC++とPythonのサンプルコードの場所

dianostic_updaterを使う場合の

C++とpythonのサンプルコードは下記のリンクにあります。

 

Pythonのサンプルコード

下記は最も簡単な

diagnostic_updaterの使い方のサンプルコードです。

#!/usr/bin/env python

import roslib
roslib.load_manifest('diagnostic_updater')
import rospy
import diagnostic_updater
import diagnostic_msgs

temperature = 0

def check_temprature(stat):

    #エラー判定
    if temperature < 10:
        stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK, "Temperature OK")
    else:
        stat.summary(diagnostic_msgs.msg.DiagnosticStatus.WARN, "Too high")

    #エラー情報追加
    stat.add("Top-Side Margin", 10 - temperature)
    return stat


if __name__=='__main__':
    rospy.init_node("diagnostic_updater_example2")

    # Updaterオブジェクト作成
    updater = diagnostic_updater.Updater()

    # ハードウェア名の設定
    updater.setHardwareID("Sensor1")

    #エラー判定関数の追加
    updater.add("upper-temperature",check_temprature)

    while not rospy.is_shutdown():
        
        temperature+=0.1
        if temperature>=20:
            temperature=0

        print temperature

        #Error チェック
        updater.update()

        rospy.sleep(0.1)

 

updaterオブジェクトを作成し、

ハードウェア名を追記した後は、

エラー判定を実施する関数を作成し、

その関数オブジェクトをaddし、

メインループでupdateすれば、

自動で/diagnostic トピックをpublishしてくれます。

 

リアルタイムにエラーデータをカテゴリー分けや解析するツール: diagnostic_aggregator

diagnostic_aggregatorは、

diagnostic_updaterによりpublishされた

DiagnosticArrayのデータをカテゴリ分けし、

/diagnostics_agg というトピックで再パブリッシュをする

ROSノードツールです。

 

前述のdiagnosticメッセージは、

対象とするハードウェアの数が少ない時は

問題がありませんが、

監視するハードウェアの数が増えていくると、

管理しずらくなります。

 

そこで、このdiagnostic_aggregatorを使って、

カテゴリー分類することにより、

沢山のエラーデータを管理しやすくなります。

 

また、diagnostic_agggregatorにより

送信されたデータは

後述するrqt_robot_monitorで分かりやすく

可視化することができます。

 

diagnostic_agggregatorの使い方

基本的には、

diagnosticメッセージのカテゴリ分けを事前に

yamlファイルに記述しておき、

そのyamlファイルを読み込んで、

aggregatorノードを起動することで、

カテゴリー分けされた/diagnostic_aggというトピックが出力されます。

 

yamlファイルは下記のように記述します。

analyzers:
  temperature:
    type: GenericAnalyzer
    path: upper-temperature

まずはじめにエラーカテゴリを設定し(templeture)

あとは、それぞれのdiagnosticメッセージを登録します。

typeは、先ほど紹介したdiagnostic_updaterを使っている場合は、

GenericAnalyzerでOKです。

pathはdiagnosticメッセージの名前を設定します。

 

あとは、

下記のようにdiagnostic_aggregatorノードを起動し、

先ほどのyamlファイルを読み込むようにすればOKです。

<launch>
  <node pkg="diagnostic_aggregator" type="aggregator_node"
    name="diagnostic_aggregator" >
    <!-- Load the file you made above -->
    <rosparam command="load" file="$(find yourpackage)/launch/analyzerSample.yaml" />
  </node>
</launch>

 

diagnostic_aggregatorのより詳しい説明は

下記を参考にすると良いと思います。

 

エラーデータを可視化するツール: rqt_robot_monitor, rqt_runtime_monitor

diagnosticのデータを簡単に確認するツールとしては、

rqt_runtime_monitorとrqt_robot_monitorがあります。

 

rqt_runtime_monitor

f:id:meison_amsl:20151219162640p:plain

rqt_runtime_monitorは、

diagnostic_msgs/DiagnosticArrayのデータを表示する

rqtプラグインです。

 

上記のスクリーンショットのように、

各ステータスのメッセージの個数を表示し、

各ステータスを開くことで、

各メッセージや、その他の詳細情報を表示させることができます。

 

rqt_robot_monitor

f:id:meison_amsl:20151220101741p:plain

 

rqt_robot_monitorは前述した

diagnostic_agggregatorのカテゴリーエラーデータを

可視化するツールです。

 

冒頭の写真のように、各エラーメッセージや、

時刻ごとのエラー、ワーニング発生状況を確認することができます。

 

diagnosticsの情報をかっこ良くrvizに可視化するプラグイン: OverlayDiagnostic

f:id:meison_amsl:20151113143212p:plain

 

下記の記事で紹介されているrvizのプラグイン

OverlayDiagnosticを使うと、

diagnosticsのメッセージをかっこ良くrvizに表示することができます。

myenigma.hatenablog.com

 

下記のようにDiaplayPanelでDiagnositcArrayのトピック名(Topic)と、

Arrayの中で表示させたいステータス情報(namespace)を指定すると、

diagnosticのステータス(Error, Warn, OK)と、

ステータスメッセージを表示することができます。

またメッセージの表示する場所を指定することもできます。

f:id:meison_amsl:20151113143235p:plain

 

ステータス情報が色で表示されたり、

ステータスメッセージが流れるように表示されたりするので

非常にかっこいいです。 

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com