ROSパッケージの依存関係をちゃんと書く

皆さんはROSパッケージの依存関係をちゃんと書いていますか?依存関係を正しく書いておけば別環境に移行してもちゃんと動くパッケージを作ることができるほか、依存パッケージのアップデートを一括で取り込む事も出来ちゃいます。package.xmlと.rosinstallを書いてsudo apt installから解放されましょう。

rosdep

多くのROSパッケージ(特にROS公式パッケージ)はrosdepで依存パッケージをインストールするのが最も簡単です。rosdepはpackage.xmlに書かれたdependタグからパッケージ名を特定して必要なものをapt installします。

package.xmlを書く

package.xmlはパッケージの名前や管理者の情報、そして依存関係を記述するためのROS用ファイルです。書き方は以下のような感じです。

<?xml version="1.0"?>
<package format="2">
  <name>my_package</name>
  <version>0.0.0</version>
  <description>The package to calculate 3D pose.</description>

  <maintainer email="ppdr@xxx.com">ppdr</maintainer>

  <license>MIT</license>

  <buildtool_depend>catkin</buildtool_depend>
  <depend>roscpp</depend><!-- ビルド時&実行時に依存するパッケージ -->
  <build_depend>eigen_conversions</build_depend><!-- ビルド時に依存するパッケージ -->
  <build_depend>my_other_package</build_depend><!-- ビルド時に依存するパッケージ -->
  <exec_depend>rospy</exec_depend><!-- 実行時に依存するパッケージ -->
  <exec_depend>python3-scipy</exec_depend><!-- 実行時に依存するパッケージ -->

  <export>
  </export>
</package>

前半はパッケージに関する情報を記述している部分です。後半のこの部分で依存関係を定義しています。

<buildtool_depend>catkin</buildtool_depend>
  <depend>roscpp</depend><!-- ビルド時&実行時に依存するパッケージ -->
  <build_depend>eigen_conversions</build_depend><!-- ビルド時に依存するパッケージ -->
  <build_depend>my_other_package</build_depend><!-- ビルド時に依存するパッケージ -->
  <exec_depend>rospy</exec_depend><!-- 実行時に依存するパッケージ -->
  <exec_depend>python3-scipy</exec_depend><!-- 実行時に依存するパッケージ -->

<build_depend>はビルド時の依存でC++ライブラリ等が該当します。<exec_depend>は実行時の依存でpythonライブラリ等が該当し、<depend>は<build_depend>と<exec_depend>の両方を記述したのと同じ効果があります。

rosdepではこれらは特に区別しないので、どれかに書いておけばインストールしてくれます。

<depend>のパッケージ名は何を書いたら良い?

ここにはrosdepで管理されているパッケージ名を書く必要がありますが、管理されているパッケージ名はrosdep dbコマンドで一覧取得できます。例えばvelodyneパッケージのrosdepでのパッケージ名を知りたい場合は、

rosdep db | grep velodyne

のようにすることで分かります。velodyneの場合は

velodyne -> ros-noetic-velodyne
velodyne_driver -> ros-noetic-velodyne-driver
velodyne_laserscan -> ros-noetic-velodyne-laserscan
...

のような出力が表示されるので、velodyneやvelodyne_driverを<depend>タグに書けばOKです。ちなみに右側の表示は環境によって異なり、noeticならros-noetic-xxx、melodicならros-melodic-xxxのように環境ごとにインストールすべきパッケージを判断してくれます。

rosパッケージだけでなく、libyaml-devやpython3-scipyなどもrosdepでインストールすることができ、UbuntuやFedora、Debianなど環境ごとに適切なパッケージを判断してくれるのでとても便利です。

なお、管理されているパッケージの一覧はここでも見ることができます。
base.yaml

rosdep updateとrosdep install

package.xmlを書いたらrosdepによるパッケージのインストールが可能です。パッケージリストのアップデートはrosdep updateコマンド、パッケージの新規インストールorアップグレードはrosdep installコマンドで行います。

# リストのアップデート
rosdep update

# パッケージインストール
rosdep install -iry --from-paths .

rosdep installコマンドはパッケージを指定してインストールすることもできますが、新しくクローンしてきたパッケージとかだとrospackリストに登録されていないと使えないので、今いるディレクトリ以下の全てのpackage.xmlに対して適用するオプションである”–from-paths .”をよく使います。

またオプション”-iry”は「ローカルにインストールされているパッケージ(自作パッケージ等)を無視する」「エラーがあっても中断しない」「インストール時の質問にはyesで答える」の意味です。

package.xmlに記述されているパッケージのインストールが全て完了したら、

#All required rosdeps installed successfully

が表示されると思います。これでrosdepによるパッケージインストールは完了です。

.rosinstall

ではrosdepによる管理がされていないパッケージを使いたい場合はどうしたら良いでしょうか。例えばDENSO製ロボット用のパッケージであるdenso_robot_rosパッケージはrosdep dbでは出てこず、githubから直接cloneしなければいけません。このようなgit cloneで取得する依存パッケージの記述方法として、.rosinstallファイルとvcstoolによるインストールがあります。

【Arduino】センサのノイズをデジタルフィルタで低減する

Arduinoでセンサの値やアナログ電圧を読み取るとき、ノイズにより値がガタついてしまうことがあります。今回はこのようなノイズをソフト(プログラム)で低減させる方法をご紹介します。

ノイズフィルタの種類

フィルタにはどのような信号をフィルタリングするかによって以下のような分け方があります。

  • ローパスフィルタ
    信号の低周波成分のみを通過させ、高周波成分を低減します。低いものだけを通すのでロー(low)パス(pass)フィルタです。センサ値はゆっくり変化しますが、ノイズは高周波であることが多いのでこのフィルタをよく使います。
  • ハイパスフィルタ
    信号の高周波成分のみを通過させ、低周波成分を低減します。オーディオ信号や交流電流等、変化する信号を取り出したいときに使います。
  • バンドパスフィルタ
    高周波と低周波の両方を低減し、真ん中の周波数帯だけ通したいときに使います。

本記事ではセンサ値のノイズを除去したいという目的なので、ローパスフィルタについてご紹介します。

FIRフィルタとIIRフィルタ

デジタルフィルタは実装の仕方にも種類があり、FIRフィルタとIIRフィルタに分けられます。FIRフィルタもIIRフィルタも係数を適切に選ぶことでローパス/ハイパス/バンドパスのいずれにも使えます。

FIRフィルタとIIRフィルタの違いについて調べたところ、大体次のような特徴があるようです。

FIRフィルタ

  • 波形が変形しない(群遅延が一定である)
  • 必ず安定である(出力が発散しない)
  • 通過域と減衰域の境目が緩やかである
  • 高性能にしたければ演算量が多くなる
  • カットオフ周波数が低い場合、性能が悪くなる

IIRフィルタ

  • 少ない演算量でも通過域と減衰域がはっきり分かれる
  • 通過域の信号をほとんど変化させない(通過域のゲインが0)
  • サンプリング周波数に対してカットオフ周波数が低い場合もフィルタリングが可能
  • 不安定なことがある(出力が振動/発散する)
  • 波形が変化する(群遅延が一定でない)

波形が変化するとは、信号の周波数によって遅延時間が変化するということで、それがあると例えばセンサ値がゆっくり変化したときは遅延が少ないが、早く変化したときは遅延が大きいということが起こりえます。

通過域と減衰域の境目がはっきりしていないと、例えば30Hz以上の信号を遮断するローパスフィルタを設計しても、40Hzの信号を50%だけ通過させてしまうといったことが起こります。

FIRフィルタを使うかIIRフィルタを使うかは、メリット・デメリットをよく考えて使う必要があるそうです。(参考1)

よくあるローパスフィルタの実装(RCフィルタ)

Arduino関連の記事でセンサ値のノイズを除去する方法として紹介されているのがRCフィルタです(参考2、参考3)。IIRフィルタに分類されるフィルタです。実装が簡単ですが性能はそこそこで、波形の変形も起こりますし通過域と減衰域の境目も緩やかです。

RCフィルタを実装して、加速度センサの出力をフィルタリングしてみました。ノイズは低減できましたが、もとの信号の波形が一部変わってしまっています。

#include <ADXL345.h>

double x_rc = 0;
ADXL345 adxl;  // 加速度センサ:ADXL345

void setup() {
    Serial.begin(9600);
    adxl.powerOn();  // 加速度センサ起動
}

void loop() {
    int x, y, z;
    adxl.readXYZ(&x, &y, &z);  // 加速度センサのxyz軸の値を読み取り、変数に格納する

    x_rc = x_rc * 0.85 + x * 0.15;  // RCフィルタ

    // 結果を出力
    Serial.print(x);  // フィルタ前
    Serial.print(" , ");
    Serial.print(x_lpf);  // フィルタ後
    Serial.println("");
    delay(10);
}
青:センサ出力 赤:RCフィルタ後

フィルタ後の波形は振幅がもとの信号よりも小さくなってしまいました。また分かりづらいですが遅延時間が所々変わっています。

(準備)ライブラリのインストール

本記事では以下のライブラリを使用します。Arduinoのライブラリ管理ツールから検索しても出てこないので、git cloneするか、zipファイルをダウンロードしてArduinoのlibrariesフォルダに展開します。

// git cloneする場合
c/Users/ppdr/Documents/Arduino/libraries$ git clone https://github.com/tttapa/Filters.git

FIRフィルタでより高性能なノイズ除去

FIRフィルタを設計してセンサ値をフィルタリングするとどうなるか見てみます。FIRフィルタの設計は本来難しい理論があるらしいのですが、ここは周波数等の特性を入れるだけでFIR/IIRフィルタの設計を行ってくれる以下のサイトを利用しました。

石川高専 山田洋士 研究室ホームページ Digital Filter Design Services

#include <ADXL345.h>
#include <FIRFilter.h>

// 設計サイトで求めたフィルタの係数
// フィルタ長=31、正規化遮断周波数=0.1
const double a[] = {
    6.237074932031003e-19,  1.203468256554930e-03,  2.789059944207731e-03,  4.234500608097008e-03,
    3.949464324766553e-03,  -2.416866536162014e-18, -8.270810074278890e-03, -1.861479317048980e-02,
    -2.543297162983507e-02, -2.127139910497268e-02, 6.003184622079840e-18,  3.965539443147850e-02,
    9.204504649186659e-02,  1.453456837490316e-01,  1.852171297065770e-01,  2.000000000000000e-01,
    1.852171297065770e-01,  1.453456837490316e-01,  9.204504649186659e-02,  3.965539443147850e-02,
    6.003184622079840e-18,  -2.127139910497268e-02, -2.543297162983507e-02, -1.861479317048980e-02,
    -8.270810074278890e-03, -2.416866536162014e-18, 3.949464324766553e-03,  4.234500608097008e-03,
    2.789059944207731e-03,  1.203468256554930e-03,  6.237074932031003e-19};

ADXL345 adxl;      // 加速度センサ:ADXL345
FIRFilter fir(a);  // FIRフィルタ

void setup() {
    Serial.begin(9600);
    adxl.powerOn();  // 加速度センサ起動
}

void loop() {
    int x, y, z;
    adxl.readXYZ(&x, &y, &z);  // 加速度センサのxyz軸の値を読み取り、変数に格納する

    // 結果を出力
    Serial.print(x);  // フィルタ前
    Serial.print(",");
    Serial.print(fir.filter(x));  // フィルタ後
    Serial.println("");
    delay(10);
}
青:センサ出力 赤:FIRフィルタ後

RCフィルタに比べて波形の変形が少なくなりました。しかし、元波形よりも振幅が小さくなってしまい、遅延が出てしまいました。これはカットオフ周波数が低い場合、性能が低くなるというFIRフィルタの特徴になります。ちなみにこの遅延時間は設計段階で知ることができ、今回は15サンプル遅延でサンプリング間隔は10msなので150msの遅れです。

IIRフィルタで遅延が少ないフィルタを実現する

FIRフィルタでは波形は変形しませんでしたが、遅延が大きくなってしまいました。また元波形よりも振幅が小さくなってしまいました。

この点を改善するため、低いカットオフ周波数に対しても遅延が少ないIIRフィルタを設計します。またIIRフィルタの特性の一つであるバタワース特性では通過域のゲインがほぼ0なので、波形の振幅を変えてしまうことも防げます。

それでは実装と結果です。

#include <ADXL345.h>
#include <IIRFilter.h>

// 設計サイトで求めたフィルタの係数
// バタワース特性、サンプリング周波数=100、パスバンド周波数=5、ストップバンド周波数=30
// 注意:設計サイトはb[1]以降の値なので、b[0]=1を追加しています
const double a[] = {2.87258084601274277e-01, 5.74516169202548554e-01, 2.87258084601274277e-01};
const double b[] = {1, -1.27955098077267904e+00, 4.77550934586849885e-01};
const double k = 1.72318869709970568e-01;

ADXL345 adxl;         // 加速度センサ:ADXL345
IIRFilter iir(a, b);  // IIRフィルタ

void setup() {
    Serial.begin(9600);
    adxl.powerOn();  // 加速度センサ起動
}

void loop() {
    int x, y, z;
    adxl.readXYZ(&x, &y, &z);  // 加速度センサのxyz軸の値を読み取り、変数に格納する

    // 結果を出力
    Serial.print(x);  // フィルタ前
    Serial.print(",");
    Serial.print(iir.filter(x * k));  // フィルタ後
    Serial.println("");
    delay(10);
}
青:センサ出力 赤:IIRフィルタ後

IIRフィルタでは遅延がかなり少なくなりました。また、振幅も元波形とほとんど同じです。

今回のようにセンサ出力が10Hz以下くらいであれば、IIRフィルタが遅延/波形の変形の両方で有利なようです。ただ、FIR/IIRのどちらが良いかは一概には言えないので、色々なパラメータで試してみる必要がありそうだと感じました。

ここまで読んでいただきありがとうございました。今回は以上です。

参考

  1. FIRとIIRの選択 – Allisone
  2. Arduinoでローパスフィルタ(センサのノイズを無くそう)
  3. ArduinoでSTEM教育​ 基礎編:デジタル入力フィルタ
  4. Arduinoで信号処理をする上で便利なフィルタのライブラリ

PythonでGoogle Homeを喋らせる(2021年最新)

PythonでGoogle Homeに任意の言葉を喋らせる方法です。(正確には音声ファイル(.mp3や.wavなど)をGoogle Homeに再生させる方法)。pychromecastというPythonモジュールを使えば簡単ですが、2020/7/20からバージョンが変わったらしく、IPアドレスでGoogle Homeに接続する方法が使えなくなっていたので最新の方法をご紹介します。

Google Homeの名前からIPアドレスを調べて接続(2020/7/20以前の方法)

pychromecast ver7.2.0以前では、まずLAN内に存在するcastデバイス(Google Homeなど)の一覧を取得し、その中から探したいデバイスの名前を検索してIPアドレスを取得し、IPアドレスでGoogle Homeに接続していました。参考までにその方法を載せておきます。

#!/usr/bin/python3
# coding:utf-8
import pychromecast
import sys


def main():
    # LAN内に存在するcastデバイスの一覧を取得
    chromecasts = pychromecast.get_chromecasts()

    # castデバイスが見つからない場合、終了
    if len(chromecasts) == 0:
        print("Google Homeが見つかりませんω")
        sys.exit(1)

    # 名前が"Living room"のデバイスを探す
    googleHome = next(
        cc for cc in chromecasts if cc.device.friendly_name == "Living room")

    # ver7.2.0以降ではgoogleHome.hostが削除されているためエラー
    print("IP address: ", googleHome.host)

    # 喋らせる
    googleHome.wait()
    mc = googleHome.media_controller
    voice_url = "http://address/to/voice/file"

    # 他の音楽を再生中の場合
    if mc.status.player_is_playing:
        print("Music is running. Stop music.")
        mc.stop()
        mc.block_until_active(timeout=30)

    # 音声を再生する
    print("Playing ", voice_url)
    mc.play_media(voice_url, "audio/mp3")


if __name__ == "__main__":
    main()

IPアドレスが分かっている場合は、

googleHome = pychromecast.Chromecast("192.168.0.10")

とすればGoogle Homeに接続できますが、わからない場合やルーターを再起動する等でIPアドレスが変わった場合は上のコードを使う必要がありました。

デバイス名で接続(2020/7/20以降)

pychromecastの最新バージョンでは、IPアドレスを使わずにGoogle Homeに接続するように変更されていました。

def main():
    # Chromecastデバイス名で検索する
    chromecasts, browser = pychromecast.get_listed_chromecasts(friendly_names=[             "Living room"])
    # castデバイスが見つからない場合、終了
    if len(chromecasts) == 0:
        print("Google Homeが見つかりません")
        sys.exit(1)

    # 最初に見つかったもの(同名のデバイスが複数あった場合)
    googleHome = chromecasts[0]

    '''
    以降、同じ
    '''

以前のバージョンだと、castデバイス一覧の取得に10秒ほどかかっていた印象ですが、この方法では1秒もかからずcastデバイスを見つけてきます。
接続高速化のために過去に接続したGoogle HomeのIPアドレスをファイルに保存していましたが、もう必要なさそうです。とりあえず時報やニュースを読み上げさせて遊んでいます。

juliusの辞書を作って認識速度&精度を爆上げする(簡単辞書作成ツール公開中)

結論

juliusのユーザ辞書作成が地味に面倒くさいので、言葉を入れたら辞書を生成してくれるサービスを作りました。

→julius辞書簡単作成サービス

なぜ作ろうと思ったか

名古屋工業大学が公開している音声認識ソフトのjuliusは、インストール時の辞書を使えば何も設定しなくともほとんどの言葉を認識してくれます。しかし沢山の言葉の中から文章を推定するため声を発してから結果が出るまで時間がかかり、また認識率もあまりよくありません。

そこで、認識したい言葉が決まっている場合には、ユーザが自分で言葉のリストを作ることができるようになっています。

やり方は公式ドキュメント等を見ればわかりますが、読み方をローマ字で指定したりと割と面倒くさいです。

書きかけです<(_ _)>

【ROS】TFのbroadcast,lookupと型変換まとめ(C++)

目次

C++におけるTF型

ROSで使うTF型は一つではなく、TF,TF2,geometry_msgsの3つのクラスが使われている。TFクラスは古いため現在使用が推奨されていないので、新しいクラスであるTF2を可能な限り使う。

よく使うTFメッセージは以下のようなものがある。

  • tf2::Transform
    変換や逆行列の計算に便利な型
  • tf2::StampedTransform
    tf2::Transformにタイムスタンプがついた型
  • geometry_msgs::Transform
    ROSトピックでよく使われる型
  • geometry_msgs::TransformStamped
    上記にタイムスタンプがついた型

TFの型まとめ

TFとしてbroadcastするにはタイムスタンプ型でなければならないが、tf2にはStampedTransform型が無いので注意。

クラスタイムスタンプなしタイムスタンプ付き内部で保持している値
tfTransformStampedTransform回転行列(3×3)+並進ベクトル(3×1)
tf2Transformなし(tf2::Stamped<Transform>はあるが、child_frameを記述できない)回転行列(3×3)+並進ベクトル(3×1)
geometry_msgsTransformTransformStampedクォータニオン(x,y,z,w)+並進ベクトル(x,y,z)

準備(CMakeLists.txt, package.xml, ヘッダファイル)

ROSで使うため、CMakeLists.txt, package.xmlに以下の記述が必要。

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  ︙
  tf2
  tf2_ros
  tf2_geometry_msgs
)

︙

catkin_package(
  INCLUDE_DIRS
  LIBRARIES tf_test
  CATKIN_DEPENDS roscpp ... tf2 tf2_ros tf2_geometry_msgs
  #DEPENDS system_lib
)

package.xml

<package format="2">
  <name>tf_test</name>
   ︙
  <!--   <doc_depend>doxygen</doc_depend> -->
  <buildtool_depend>catkin</buildtool_depend>

  <depend>roscpp</depend>
  <depend>rospy</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

ヘッダファイル

大体以下があれば大丈夫なはず。

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

TF Broadcaster

TFクラスとTF2クラスのbroadcasterの違い

TFを発信する際はタイムスタンプ付きの型(geometry_msgs::TransformStamped等)を使う。各クラスのBroadcasterが発信できる型は以下の通り。

tf::StampedTransformgeometry_msgs::TransformStamped
tf::broadcaster
tf2::broadcaster×
broadcastできる型の一覧

現在はtfクラスを使うことは少ないと思われるので、以降はtf2を例に説明します。

基本のbroadcaster

tf2::broadcasterはgeometry_msgs::TransformStamped型しか発信できないため、tf2::Transform型等を発信したければ一度geometry_msgs::TransformStampedに変換する必要がある。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

void pub_tf(const geometry_msgs::TransformStamped trans_msg){
  // geometry_msgs::TransformStamped型のTFを発信
  static tf2_ros::TransformBroadcaster br;

  br.sendTransform(trans_msg);
}

void pub_tf(const tf2::Transform trans_tf2){
  // tf2::Transform型のTFを発信
  // tf2::Transform型は姿勢しか持っていないので、タイムスタンプやフレーム名を追加する必要がある
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped trans_msg;
  trans_msg.header.stamp = ros::Time::now();
  trans_msg.header.frame_id = "map";
  trans_msg.child_frame_id = "robot";
  trans_msg.transform = tf2::toMsg(trans_tf2);
  br.sendTransform(trans_msg);
}

なお、関数の最初で

static tf2_ros::TransformBroadcaster br;

という部分があるが、恥ずかしながらstaticを付ける効果がわからなかったので調べた。どうやらstaticを付けた変数は関数の実行時に毎回初期化されるのではなく、最初の一回だけ初期化された後は関数を抜けても残り続けるらしい。高速化のためなのか必ず必要なのかわからないが、つけておいたほうが良いと思う。

複数のTFを一度に発信する

発信できる型ならば、std::vectorで複数をまとめて同時に発信することも可能。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

void publish_all(){
  // tf2でgeometry_msgsのTFを複数まとめて送信する例
  static tf2_ros::TransformBroadcaster br;

  geometry_msgs::TransformStamped tf1, tf2, tf3;
  std::vector<geometry_msgs::TransformStamped> all_tf;

  all_tf.push_back(tf1);
  all_tf.push_back(tf2);
  all_tf.push_back(tf3);

  br.sendTransform(all_tf);
}

TF listener

TFは現在の値を取得することはできない(ツリー状につながったTFがそれぞれ一定周期で更新されるため、現時点よりもちょっと古い情報しか持っていない)ため、二通りの方法がある。

下記のサイトがとても参考になった。

tf,tf2完全理解

方法① すべてのTFが更新されるまで待ってから取得する(基本)

現時点でのparentからchildまでのTFを取るために、その間のすべてのTFが更新されるのを待って取得する。この方法ではTFの時間的な補間がされるため正確なTFが取得できるが、現時点から少し経ってからしか結果が得られない。

bool get_tf(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame, const float timeout){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // 時刻を記録する
  ros::Time now = ros::Time::now();

  // すべてのTFが補間可能になるまで待つ
  if(!tfBuffer.canTransform(parent_frame, child_frame, now, ros::Duration(timeout))){
    ROS_WARN("Could not lookup transform from %s to %s, in duration %f [sec]", parent_frame.c_str(), child_frame.c_str(), timeout);
    return false;
  }

  // nowの時刻でのtransformを取得する
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, now, ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

方法② とにかく今のTFを取得する

とにかく今登録されている情報を使ってparentからchildまでのTFを取得する。この方法では即座に結果が帰ってくるが、parentからchild間に存在するそれぞれのTFが更新されたタイミングはバラバラなため、時間的にずれたTFをつなぐことになり正確ではない。

bool get_tf_immediate(geometry_msgs::TransformStamped& tf, const std::string parent_frame, const std::string child_frame){
  // ここは良くない。tfBufferとtfListenerは関数の外で宣言し、できるだけ生存期間を長く取るほうが良い。(tfを常にlistenするため)
  static tf2_ros::Buffer tfBuffer;
  static tf2_ros::TransformListener tfListener(tfBuffer);

  // ros::Time(0)=parentとchildの最新TFが登録された時刻のうち早い方
  // とりあえず今登録されているTFを取得するが、古い情報を使うため現在のTFとは異なる
  try{
    tf = tfBuffer.lookupTransform(parent_frame, child_frame, ros::Time(0), ros::Duration(0.1));
    return true;
  }
  catch(tf2::TransformException &ex){
    ROS_WARN("%s", ex.what());
    return false;
  }
}

TFの型変換

tf以外のもの→TF型

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
TF型以外からTF型を作る場合

TF型→他のTF型、逆方向、合成等

tf::Transformtf2::Transformgeometry_msgs::Transform
xyzとロール、ピッチ、ヨー
xyzとクォータニオン
変換行列(4×4)
tf::Transform
tf2::Transform
geometry_msgs
逆方向のTF(なし)
2つのTFの合成(なし)
2つのTFの差分(なし)
TF型を変換したり、データを取り出す場合

xyzとロール、ピッチ、ヨーからgeometry_msgs

ロール、ピッチ、ヨーから直接geometry_msgs::Quaternionを作る関数は無いので、tf::Quaternionで作ってから代入する。

geometry_msgs::Transform xyz_rpy_to_geometry(double x, double y, double z, double roll, double pitch, double yaw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

xyzとクォータニオンからgeometry_msgs

xyzとクォータニオン(x,y,z,w)が分かっている場合、geometry_msgs::Transformにそのまま代入するだけ。

geometry_msgs::Transform xyz_quat_to_geometry(double x, double y, double z, double qx, double qy, double qz, double qw){
  geometry_msgs::Transform gt;
  gt.translation.x = x;
  gt.translation.y = y;
  gt.translation.z = z;
  gt.rotation.x = qx;
  gt.rotation.y = qy;
  gt.rotation.z = qz;
  gt.rotation.w = qw;

  return gt;
}

変換行列からgeometry_msgs

あまり使うことは無いかもしれないが、変換行列を並進ベクトル(3次元ベクトル)と回転行列(3×3行列)で表していた場合、geometry_msgsを作るにはこうする。

  geometry_msgs::Transform matrix_to_geometry(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  geometry_msgs::Transform gt;
  gt.translation.x = trans.getX();
  gt.translation.y = trans.getY();
  gt.translation.z = trans.getZ();
  tf2::Quaternion q;
  rot.getRotation(q);
  gt.rotation.x = q.x();
  gt.rotation.y = q.y();
  gt.rotation.z = q.z();
  gt.rotation.w = q.w();

  return gt;
}

geometry_msgsからxyzとロール、ピッチ、ヨー

geometry_msgsからロール、ピッチ、ヨーを直接得ることはできないので、geometry_msgs::Quaternion→tf::Quaternion→tf::Matrix3x3に変換してからgetRPY関数で取得する。

std::tuple<double, double, double, double, double, double> geometry_to_xyz_rpy(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;

  double roll, pitch, yaw;
  tf::Quaternion q(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);

}

geometry_msgsからxyzとクォータニオン

geometry_msgsはxyzとクォータニオンを持つ型なので、そのまま取得する。

std::tuple<double, double, double, double, double, double, double> geometry_to_xyz_quat(geometry_msgs::Transform gt){
  double x = gt.translation.x;
  double y = gt.translation.y;
  double z = gt.translation.z;
  double qx = gt.rotation.x;
  double qy = gt.rotation.y;
  double qz = gt.rotation.z;
  double qw = gt.rotation.w;

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

geometry_msgsから変換行列

geometry_msgsから並進ベクトル(3次元)と回転行列(3×3行列)に変換する。

std::tuple<tf2::Matrix3x3, tf2::Vector3> geometry_to_matrix(geometry_msgs::Transform gt){
  tf2::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf2::Matrix3x3 rot(tf2::Quaternion(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w));

  return std::make_tuple(rot, trans);
}

geometry_msgsからtf::Transform

tf::Transform geometry_to_tf(geometry_msgs::Transform gt){
  tf::Vector3 trans(gt.translation.x, gt.translation.y, gt.translation.z);
  tf::Quaternion quat(gt.rotation.x, gt.rotation.y, gt.rotation.z, gt.rotation.w);
  tf::Transform transform(quat, trans);

  return transform;
}

geometry_msgsからtf2::Transform

tf2では便利なconvert関数が使えるので、シンプルに変換できる。

tf2::Transform geometry_to_tf2(geometry_msgs::Transform gt){
  tf2::Transform tf2;
  tf2::convert(gt, tf2); // geometry_msgs::Transform -> tf2::Transform

  return tf2;
}

geometry_msgsの逆方向TF

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの合成

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

geometry_msgsの差分

geometry_msgsのままではできないので、一旦tf2::Transformに変換する。

xyzとロール、ピッチ、ヨーからtf2::Transform

コンストラクタを使ってtf2::Quaternion q(roll, pitch, yaw)とせず、一旦tf2::Quaternion q;で作ってからsetRPY関数で設定していることに注意。

tf2::Transform xyz_rpy_to_tf2(double x, double y, double z, double roll, double pitch, double yaw){
  tf2::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf2::Transform tf2(q, tf2::Vector3(x, y, z));

  return tf2;
}

xyzとクォータニオンからtf2::Transform

クォータニオンから作るときはtf2::Quaternionのコンストラクタを使える。

tf2::Transform xyz_quat_to_tf2(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

変換行列からtf2::Transform

これは簡単。

tf2::Transform matrix_to_tf2(tf2::Matrix3x3 rot, tf2::Vector3 trans){
  tf2::Transform tf2(rot, trans);
  return tf2;
}

tf2::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf2::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf2_to_xyz_rpy(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double roll, pitch, yaw;
  tf2::Quaternion q = tf2.getRotation();
  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf2::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

  std::tuple<double, double, double, double, double, double, double> tf2_to_xyz_quat(tf2::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();

  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf2::Transformから変換行列

とてもシンプル。

std::tuple<tf2::Matrix3x3, tf2::Vector3> tf2_to_matrix(tf2::Transform tf2){
  tf2::Matrix3x3 rot = tf2.getBasis();
  tf2::Vector3 trans = tf2.getOrigin();

  return std::make_tuple(rot, trans);
}

tf2::Transformからtf::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf::Transform tf2_to_tf(tf::Transform tf2){
  double x = tf2.getOrigin().x();
  double y = tf2.getOrigin().y();
  double z = tf2.getOrigin().z();
  double qx = tf2.getRotation().x();
  double qy = tf2.getRotation().y();
  double qz = tf2.getRotation().z();
  double qw = tf2.getRotation().w();

  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

tf2::Transformからgeometry_msgs

とてもシンプル。

geometry_msgs::Transform tf2_to_geometry_msgs(tf2::Transform tf2){
  geometry_msgs::Transform gt = tf2::toMsg(tf2);
  return gt;
}

tf2::Transformの逆方向TF

TF2使っててよかった!

tf2::Transform tf2_invert(tf2::Transform tf2){
  return tf2.inverse();
}

tf2::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf2::Transform tf2_add(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_A* tf2_B;
}

tf2::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf2::Transform tf2_sub(tf2::Transform tf2_A, tf2::Transform tf2_B){
  return tf2_B * tf2_A.inverse();
}

xyzとロール、ピッチ、ヨーからtf::Transform

tf2::Transformの場合とほぼ同様のやり方でできる。

tf::Transform xyz_rpy_to_tf(double x, double y, double z, double roll, double pitch, double yaw){
  tf::Quaternion q;
  q.setRPY(roll, pitch, yaw);
  tf::Transform t(q, tf::Vector3(x, y, z));

  return t;
}

xyzとクォータニオンからtf::Transform

クォータニオンから作るときはtf::Quaternionのコンストラクタを使える。

tf::Transform xyz_quat_to_tf(double x, double y, double z, double qx, double qy, double qz, double qw){
  tf::Transform t(tf::Quaternion(qx, qy, qz, qw), tf::Vector3(x, y, z));
  return t;
}

変換行列からtf2::Transform

これは簡単。

tf::Transform matrix_to_tf(tf::Matrix3x3 rot, tf::Vector3 trans){
  tf::Transform t(rot, trans);
  return t;
}

tf::Transformからxyzとロール、ピッチ、ヨー

一旦tf::Matrix3x3を作って、getRPYで取り出すしか無いっぽい。tf::Quaternionから直接作る等、もっとシンプルなり方をご存知の方がいましたらコメントで教えて下さい。

std::tuple<double, double, double, double, double, double> tf_to_xyz_rpy(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double roll, pitch, yaw;
  tf::Quaternion q = t.getRotation();
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

  return std::make_tuple(x, y, z, roll, pitch, yaw);
}

tf::Transformからxyzとクォータニオン

クォータニオンの場合はシンプル。

std::tuple<double, double, double, double, double, double, double> tf_to_xyz_quat(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();

  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  return std::make_tuple(x, y, z, qx, qy, qz, qw);
}

tf::Transformから変換行列

とてもシンプル。

std::tuple<tf::Matrix3x3, tf::Vector3> tf_to_matrix(tf::Transform t){
  tf::Matrix3x3 rot = t.getBasis();
  tf::Vector3 trans = t.getOrigin();

  return std::make_tuple(rot, trans);
}

tf::Transformからtf2::Transform

ちょっと調べた限りでは変換する関数は見つからなかった。用途も特に無いかもしれない。どうしてもやるなら、一回数値型に変えてから作れば可能。

tf2::Transform tf_to_tf2(tf::Transform t){
  double x = t.getOrigin().x();
  double y = t.getOrigin().y();
  double z = t.getOrigin().z();
  double qx = t.getRotation().x();
  double qy = t.getRotation().y();
  double qz = t.getRotation().z();
  double qw = t.getRotation().w();

  tf2::Transform tf2(tf2::Quaternion(qx, qy, qz, qw), tf2::Vector3(x, y, z));
  return tf2;
}

tf::Transformからgeometry_msgs

tf2の時と関数名が違うので注意。こういう微妙な違いがあったりするのが怖い。

geometry_msgs::Transform tf_to_geometry_msgs(tf::Transform t){
  geometry_msgs::Transform gt;
  tf::transformTFToMsg(t, gt);
  return gt;
}

tf::Transformの逆方向TF

tf::Transform tf_invert(tf::Transform t){
  return t.inverse();
}

tf::Transformの合成

2つのTFを合成する場合はその順番に注意。2つのTF(A,B)をA→Bの順番でつなげるなら座標変換の計算はA*B。

tf::Transform tf_add(tf::Transform t_A, tf::Transform t_B){
  return t_A * t_B;
}

tf::Transformの差分

A,Bという2つのTFがあったとき、AとBの差分は以下のように計算できる。

tf::Transform tf_sub(tf::Transform t_A, tf::Transform t_B){
  return t_B * t_A.inverse();
}

▲準備 ▲broadcaster ▲listener ▲変換一覧

初心者のためのIoT&電子工作入門 ①準備編

はじめに

最近スマート家電やホームコントロールという言葉もすっかり身近になってきました。声をかけるだけでテレビやエアコンがオンする環境はもう当たり前になってきています。

しかし、家電コントロールのシステムや規格はまだまだ統一されておらず、家電自体にネットワーク機能がついたものを購入するか、テレビを付けたりなどのリモコン(赤外線)操作を行うデバイスを購入するしかないのが現状です。しかも、そのスマートリモコンシステムが1万円くらいしたりするので、便利かもしれないけど購入は迷う値段です。

家の中のすべての電化製品をコントロールするためには、自分でモータやセンサを購入し、回路を作成し、基盤にはんだ付けし、プログラムを書き込むという作業が必要になってきます。本記事は「好きな家電をコントロールしてみたい」「電子工作でなにか役に立つものを作りたい」という方に向けて、やるべきことを解説していきます。

対象とする人

  • 家庭内IoTシステムを安く、簡単に作りたい方
  • 無線IoTデバイスの自作に挑戦してみたい方
  • RaspberryPiやArduinoを買ってみたは良いが、使い道に迷っている方
  • 実際に使えるプログラミングの勉強を(少しだけ)したい方

家庭内IoTシステムの完成形

現在私が家で稼働させているシステムの構成図です。

本記事で解説するシステムの完成形

本記事では①~④を中心に解説していきます。最終的には声やスマホ、時刻やセンサによってテレビやエアコンを点けたり、ライトを操作することができることを目指します。

①~④は一応優先順位です。それぞれの役割と、作るのに必要な知識・技術を簡単に解説しておきます。

①無線マイコン(ESP8266)

  • 役割:
    WiFiによる通信で動作命令を受け取ったり、一定時間経過など何らかのタイミングによって、出力ピンから電圧を出力しモータやLEDを動作させます。
  • 必要事項:
    Arduinoプログラミング(C++)、HTTP通信、電子回路(モータ、LED、抵抗、トランジスタ等)

IoTのキモとなる、プログラミングとハードウェアを結びつけるマイコン(小型コンピュータ)です。本記事ではESP8266というモジュールを使います。このモジュールは最近では有名なArduinoにWiFi通信モジュールがついたものです。かなり安価(1個700円程度)で購入でき、一つの機能に対して一つのモジュールを使うのが基本です。

②サーバ(RaspberryPi等)

  • 役割:
    マイコンではできないような、Webから天気予報の取得や画像・音声の解析を行い、必要に応じてマイコンに動作命令を送ります。また、家の外からスマホでデータを送る際、通信を受け取るための玄関的な役割(=サーバ)を担います。
  • 必要事項:
    Pythonプログラミング、Linuxの知識、ネットワーク(サーバ)の知識

マイコンでできないような画像・音声解析や、複雑なネットワーク機能を担当します。ちゃんとしたコンピュータであり、できることは無限ですが、LinuxというOSを使っているため初心者には多くの知識が必要となります。本記事ではサーバの構築など一部だけ紹介します。

③ルータ

  • 役割:
    WiFiや有線LANで繋がれた機器同士の通信経路を作ります。家の中だけで使っている場合はSSIDとパスワードを各機器に登録するだけで接続できますが、サーバで家の外からの通信を受ける場合には少し設定が必要になってきます。
  • 必要事項:
    ネットワークの知識

少し設定すれば終わりですが、家の外からの通信を受けるときは適切に設定する必要があります。

④スマートスピーカー(Google Home)

  • 役割:
    音声を検出し、予め登録した言葉を検出したら何か処理をさせます。
  • 必要事項:
    特になし

Google Homeやアレクサ等最近流行りのスマートスピーカーですが、対応機器が少ないため持て余している方もいるかと思います。本記事では自作したIoTデバイスをスマートスピーカーで操作できるようにします。

上の図では家庭内だけで通信が完結しているように書きましたが、Google Homeを使うにはIFTTTというサービスを利用するのが一番簡単です。そのためには家の外からの通信を受け入れる必要があり、②のサーバが必要になってきます(IFTTTが外部にあるため)。

できるようになること

この記事を順番に見ていくとできるようになることを段階的にまとめてみます。

1.①無線マイコンのみ

無線マイコンのみでもTVやエアコンを付けることはできます。問題はどうやって動作命令を伝えるかですが、簡単にやるならスマホでchromeなどを開いて特定のURLを打ち込むことでマイコンを動作させることができます。

また、無線マイコンにセンサを付け、無線マイコン同士で通信させることも可能です。

無線マイコンのみでできること

2.①無線マイコンと②RaspberryPi(サーバなし)

RaspberryPiを導入することで、Webから天気予報や現在の気温等を取得することができます。よって人が入力しなくても無線マイコンを動作させることが可能です。

また、RaspberryPiに音声解析ソフトをインストールすればスマートスピーカーがなくても音声で家電を操作することができます。ですが、認識精度はやはりGoogle Homeが圧倒的に高精度ですので、ここで終わるのはもったいない気がします。

無線マイコンとRaspberryPi(サーバなし)でできること

3.①無線マイコンと②サーバと③ルータを設定

RaspberryPiをサーバ化すると、今まではできなかった家の外からの通信を受けることができます。これにより外出先からスマホでエアコンを付けることが可能になります。

また、2つのサービスを結びつける「IFTTT」を使うことで、さらに動作命令を送る幅が広がります。

無線マイコン+サーバでできること

4.①無線マイコン+②サーバ+③ルータ+④スマートスピーカー

最後にスマートスピーカーを追加すれば、好きな家電を声で操作できるようになります。IFTTTを使えばスマートスピーカーの利用は簡単なので、次の最終形にするまでに時間はほとんどかからないと思います。

無線マイコン+サーバ+スマートスピーカー

今回は最終的な全体システムの説明と、それぞれの構成がどのような役割を持つのかを解説しました。次回からはESP8266を使って動作命令を受け取る→LED、モータを動かす部分を解説していこうと思います。

つたない文章でしたが、次回もよろしくお願いします。

【Arduino】加速度センサ MPU9250の使い方&測定レンジ設定

こんにちは!今回は加速度+ジャイロ+方位が取れる高精度センサであるMPU-9250の使い方をご紹介します。データ取得や測定レンジ変更など基本的な使い方と使用レビューをしようと思います。

なおセンサの詳しい仕様はこちらのデータシートを参照すれば分かります。この記事はデータシートを見るのが面倒な時のための備忘録的記事です。

この記事は自分で色々試行錯誤した結果でもあるので、間違っている箇所があればぜひご指摘ください。

目次

  • 用意するもの
  • 配線時の注意点
  • データの取得
  • 測定レンジ、内部LPFの設定

用意するもの

  • MPU-9250
    今回使用するセンサです
  • Arduino nano
    たまたま手元にあったnanoを使いましたが、Arduino Unoやラズパイなど、I2CまたはSPI通信が使えるなら何でもOKだと思います。今回はArduino系でI2Cを使う想定でサンプルコードをご紹介します。

配線時の注意

配線はデータシートの通りですが、必要最小限でこれだけになります。ポイントは左下と右下のピンで、左下はアドレス選択ピンで3.3VかGNDどちらに接続してもいいですが通信する時のアドレスが異なります。

左下ピンスレーブアドレス
LOW(0V)0x68
HIGH(3.3V)0x69

右下はI2Cの場合3.3Vに接続します。

最小構成での配線

データの取得

I2C通信するライブラリはArduinoならばWire.hが使えます。MPU-9250のデータ読み込みはデバイスアドレスを指定して、メモリアドレスを書き込んだあと、メモリを読み出すことで取得します。

加速度、ジャイロの値を取得するサンプルコードをご紹介します。データは16ビットで、上位8ビットと下位8ビットに分かれているので、データの読み込みを2回行い結合する関数i2c_read2を用意しています。この関数に読みたいメモリアドレスを入力するだけで、データを取得することが出来ます。

#include <Wire.h>

#define DEVICE_ADDRESS 0x69
#define MPU_WAKE_UP1 0x6B
#define MPU_WAKE_UP2 0x37
#define X_a 0x3B
#define Y_a 0x3D
#define Z_a 0x3F
#define X_g 0x43
#define Y_g 0x45
#define Z_g 0x47

#define ACC_RESOLUTION 32768.0
#define GYR_RESOLUTION 32768.0
#define ACC_RANGE 2.0//[m/s/s]
#define GYR_RANGE 250//[deg/s]

//メモリアドレスから2バイト読み込んで整数に直します。データはビッグエンディアンです。
short i2c_read2(char memory_address){
  //先頭アドレスの読み込み。16bitのshort型に格納。
  Wire.beginTransmission(DEVICE_ADDRESS);
  Wire.write(memory_address);
  Wire.endTransmission(false);

  Wire.requestFrom(DEVICE_ADDRESS,1);
  short d1 = Wire.read();
  Wire.endTransmission(true);

  //次のアドレスの読み込み。16bitのshort型に格納。
  Wire.beginTransmission(DEVICE_ADDRESS);
  Wire.write(memory_address + 0x1);
  Wire.endTransmission(false);

  Wire.requestFrom(DEVICE_ADDRESS,1);
  short d2 = Wire.read();
  Wire.endTransmission(true);

  //一つ目のデータを8ビットシフト。
  d1 <<=8;

  //2つのデータを結合して返す。
  return d1 | d2;
}

void setup() {
  Wire.begin();
  Serial.begin(9600);
}



void loop() {
  float acc_x = i2c_read2(X_a)/ACC_RESOLUTION * ACC_RANGE;
  float acc_y = i2c_read2(Y_a)/ACC_RESOLUTION * ACC_RANGE;
  float gyro  = i2c_read2(Z_g)/GYR_RESOLUTION * GYR_RANGE;

  Serial.println(acc_x);

  delay(10);
}

測定レンジと内部LPFの設定

このセンサは測定レンジが加速度は±2G~±16G、ジャイロは±250deg/s~±2000deg/sまで設定可能です。設定は特定のメモリアドレスにデータを書き込むことで行います。

メモリアドレスに値を書き込む関数i2c_writeを用意しました。

//メモリアドレスに値を書き込む
void i2c_write(char address, char value){
  Wire.beginTransmission(DEVICE_ADDRESS);
  Wire.write(address);
  Wire.write(value);
  Wire.endTransmission(true);
}

これを使って、次のような感じに設定を行います。メモリアドレス26~29が設定値を書き込むアドレスです。

i2c_write(26, 0b00000110);//角加速度LPF設定(Bandwidth:5Hz Delay:33.48ms)
i2c_write(27, 0b00000000);//角加速度レンジ設定([0b000??000] 00:250dps 01:500dps 10:1000dps 11:2000dps)
i2c_write(28, 0b00000000);//加速度レンジ設定([0b000??000] 00:2g 01:4g 10:8g 11:16g)
i2c_write(29, 0b00000110);//加速度LPF設定(Bandwidth:5Hz Delay:66.96ms)

結果を見てみる

X軸とY軸の加速度をプロットしてみました。縦軸が加速度[G]で横軸が時刻です。いい感じに取れてると思います!

加速度をプロットしたグラフ。縦軸が加速度[G]、横軸が時間。青がX軸、赤がY軸。

この例ではLPF設定をカットオフ周波数5Hzに設定したので、かなり強いフィルターがかかっています。それでも手で振ったくらいの加速度は問題なく表示されます。電動歯ブラシを当ててみたところ、殆ど値が変化しませんでした。ここまで強いフィルターが必要な状況はそう無いと思いますが、必要に応じて調整するのがいいと思います。

今回は以上です。

ESP8266で指パッチン検出器を作る②

前回、FFTと相関係数で指パッチンを検出する仕組みを考えました。ところが、実際使ってみると何もしていないのに作動してしまうことがたまにあり、完璧ではありませんでした。

そこで今回は、誤作動しないような判定をSVM(サポートベクターマシン)で実現した話をご紹介します。

SVM(サポートベクターマシン)とは

SVMはデータの集合に対して、データを2つに分類するような境界線(面)を引く手法です。例によって、説明は適当です。詳しく知りたい方は「SVM」で検索すれば簡単に見つかると思います。

今回はこれを使って指パッチンデータとそれ以外のデータを分離したいと思います。分離が出来たら、得られた境界面を使って新たに得られたデータが指パッチンかそれ以外のどちらのグループに所属するか判定します。

SVMは学習データから分離面を求める時こそ多少計算を必要とするものの、一度分離面が得られたら新たなデータを判定するときは単純な計算ですむという利点があります。したがって学習だけパソコンで行えば、判定するときはESP8266でも十分です。

SVMを使って分離面を引く

前回は指パッチンの正解データだけを使いましたが、今回は指パッチンでないデータも必要です。前回同様にマイク入力をサンプリング周波数8000Hz、サンプル数1024のFFTにかけたデータ(512次元)を扱います。指パッチンしたときに出力されたデータと、それ以外の例えば手を叩いたときのデータをそれぞれ100データくらい用意しました。

早速SVMを使って分離面を引きたいと思います。SVMによる学習はpythonのライブラリであるscikit-learnを使いました。

環境を整えるのも面倒なので、Google Colaboratoryを使います。Google ColaboratoryはGoogleが提供しているWeb上でコーディングできるpython環境で、必要なパッケージも全て最初から入っているのでめちゃめちゃ簡単に開発を始められます。初めて使いましたが簡単すぎて驚きました。

scikit-learnでどうやってSVMをするかは下記のサイトがとても参考になりました。

https://data-science.gr.jp/implementation/iml_sklearn_svm.html

(ほとんど上記サイトのままですが、一応僕が使ったコードは後でgitか何かで公開する予定です。)

プログラムを走らせるとサポートベクトルとモデルの精度などの情報が出力されます。

得られた分離面を使ってデータを判定する

得られた分離面を使って、指パッチン判定を行っていきます。分離面といっても上記サイトの例ではRBFカーネルを用いた非線形分離を行っていますので、得られるのは面の式ではなく何個かのサポートベクトルになりますが、この辺の詳しい説明は割愛させていただきます(正直に言うと自分がまだ十分に理解していないからです笑)

とにかく、このサポートベクトルを使って新しいデータを判定するには、次の式に代入すれば良いとのことです。

サポートベクター分類の場合,以下の式を解くことで分類が出力される.上のパラメーターにおいて .support_vectors_ は以下の \(x_i\),.dual_coef_ は \(y_i\alpha_i\),.intercept_ は \(\rho\) である.

$$\begin{eqnarray}\operatorname{sgn}(\sum_{i=1}^{n}y_i\alpha_iK(x_i,x)+\rho)\tag{1}\end{eqnarray}$$

また,カーネルの中身は今回用いた RBF の場合,以下で計算されるが,このとき用いるのが .gamma であり,これは以下の \(\gamma\) である.すなわち,上のパラメーターさえ抽出すればものすごく簡単な計算で予測結果を得ることができる.これは別のプログラミング言語で書き換える際に便利.

$$K(x,x’)=e^{(-\gamma|x-x’|^2)}\tag{2}$$

上記サイトより引用

つまり、式(1)に学習によって得られたサポートベクトルと定数、マイクから拾ったデータを代入すれば、出てきた数値の符号によって指パッチンかそうでないかを判定できます。

この式をプログラムとして実装して、実際に判定するとこんな感じになります。

縦軸がSVMの出力、横軸はデータのindex。ある程度大きい音がしたときのみ判定。

グラフは作ったSVMを実行しながら何回か物音を立ててみた結果です。マイク入力に対して常にFFT→SVMを行うと計算が追いつかないので、ある程度大きい音がしたときだけ判定を行うようにしています。

見て分かる通り、指パッチンしたときだけSVMの出力が正になっていることがわかります。自分で言うのもなんですが、かなりいい精度で検出できます。手や硬いものをぶつけても検出しませんし、後ろで音楽を流していてもちゃんと検出してくれます。たまに指パッチンしても検出しないときがありますが、何もしていないときに誤検出してしまうことはありませんでした。

指パッチン検出器、完成です!
あとはこれを使って色々なアクションを起こすデバイスを作っていく予定です。

最後までご覧いただきありがとうございました!
こんなことできたらいいな、というアイデアがありましたら是非コメントで教えて下さい。

ESP8266で指パッチン検出器を作る①

最近私生活がバタバタしていたので久しぶりの投稿です。ちょっと前から考えていた指パッチンで何かアクションを起こすデバイスが試行錯誤を重ねようやく完成したので、作り方をご紹介しようと思います。

今回の目標

今回の目標は「指パッチンしたら何か起こる」デバイスを作ることです。

  • 指パッチン「だけ」を検出して何かアクションを起こす。手を叩いたり何かがぶつかる音では反応しない。
  • できるだけ誤作動(指パッチンしていないのにアクション)しない。
  • コストと消費電力を抑えるため、ラズパイではなくESP8266を使う。そのため、C++でプログラムを書き、しかもコード量を減らす必要がある。

家に帰ってきたら指を鳴らしてカッコよく電気をつけたい。そんな要望に当システムがお応えします。

使用したセンサとマイコンボード

当記事でよく使っているESP8266モジュールと、アンプ付きマイクモジュールを使用しました。

指パッチンの音を解析する

指パッチンの音の特徴は?

指パッチンだけを検出するためには、指パッチンの音の特徴を捉える必要があります。マイクで取った音の信号だけでは、データがバラバラすぎて判定できません。

フーリエ変換してみる

指パッチンの音と手を叩く音は、同じ単発音ですが、人間の耳には音色の違いがはっきり分かります。その理由は音に含まれる周波数の配合が異なるためです。

そこでフーリエ変換を使って周波数分析を行います。周波数分析とは一定時間の音の時系列データの中に、各周波数成分がどのくらい入っているかを調べる手法です(ざっくり)。フーリエ変換を行うと、時系列データ(横軸が時間、縦軸が振幅)が周波数成分に変換されて、横軸が周波数、縦軸がパワースペクトルになります。

周波数分析を使えば指パッチンと手を叩く音で異なる結果が得られるはずです。
指パッチン、手を叩く、金属をぶつけるの3種類の音を周波数分析した結果がこちらです。

3種類の音をフーリエ変換した結果。指パッチン同士はなんとなく似てそう。

なんとなく指パッチンは似ている波形が出ています。逆にそれ以外は全く違う周波数的特徴を持っていることがわかります。次はこの指パッチンとそれ以外のデータを分類する判定アルゴリズムを作っていきます。

指パッチンの判定

周波数分析が出来たとして、どういう波形なら指パッチンと判断するかを決める必要があります。そのためには指パッチンに近いかどうかを定量的に評価する指標が必要です。評価の方法としては色々ありますが、相関係数を使う方法とサポートベクターマシンを使う方法を思いついたのでやってみました。

相関係数を使って判定

得られたデータがどれくらい指パッチンに似ているか定量的にわかればいいので、まず相関係数を使う方法を思いつきました。相関係数は2つのデータ(ベクトル)が似ているかどうかを数値で表すことが出来ます。

まず指パッチンを何回か録音して、指パッチンの平均的なデータを求めておきます。実際に判定するときは、この平均データと新たに得られたデータを比較することによって、得られたデータがどのくらい指パッチンに似ているか判定します。

指パッチンを20回録音して得られた平均データがこちらです。

指パッチン20回の平均データ。これを基準に相関係数を求める。

これと新たに得られたデータを比較して、相関係数が高ければ指パッチンと判定します。以下は指パッチンの平均データをx軸、新たに得られたデータをy軸にとった散布図です。教科書で見たことあるようなきれいなグラフになりました笑。

平均をデータに近い=相関が強い

相関係数はデータが似ているほど1に近くなります。逆に全く相関がなければ0になります。指パッチンと指パッチン以外の約100回のデータで実際に判定を行ってみたところ、次のようになりました。

約100回のデータに対して相関係数を計算。指パッチンはちゃんと相関係数が大きくなっている。

これを見ると、指パッチンのときはだいたい0.4以上の相関係数が出ていることがわかります。

これをトリガーにすれば実用的な判定システムが作れそうです。

長くなってきたので今回はここまでにします。
次回に続きます。

次回→相関係数で判定するデメリットとSVM(サポートベクターマシン)

ESP8266で時刻取得ができない

ESPで実行ログに現在時刻をつけて確認したいと思い、ネットから時刻を取得する方法を調べてみました。

こちらで紹介されているコードをそのまま使ってみて、ちゃんと取得できたので自分のプログラムに入れてみたのですが、なぜか取得できませんでした。

取得できない原因

原因は意外なところにあって、WiFi接続を固定IPアドレスにしていたからでした。ESPWiFiはIPアドレスやゲートウェイの設定はあってもDNSサーバーを設定する方法が無いらしく、固定IPだけ設定すると名前解決ができなくて取得に失敗するっぽいです。

  (中略)
  /* 
   *  固定IPに設定していると、名前解決できない!(=ドメイン名でアクセスできない)
   */
  IPAddress ip(192,168,1,33);
  IPAddress gateway(192,168,1,1);
  IPAddress subnet(255,255,255,0);
  WiFi.config(ip, gateway, subnet);
  
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  while(WiFi.status() != WL_CONNECTED) {
    Serial.print('.');
    delay(500);
  }
  Serial.println();
  Serial.printf("Connected, IP address: ");
  Serial.println(WiFi.localIP());

解決方法

解決方法としては、mDNSとかいうライブラリを使って名前解決できるようにするか、簡易的には以下のようにNTPサーバーをIPアドレスで指定すればOKです。

(中略)
//configTime( JST, 0, "ntp.nict.jp", "ntp.jst.mfeed.ad.jp");
// 名前解決できないときは直接IPアドレスでNTPサーバーを指定
configTime( JST, 0, "133.243.238.244");

これだけ。簡単。
本当はちゃんとDNS使いたいけど今の環境だと色々難しいので早く回線契約したい。