Yura YuLife

ITエンジニアの覚え書き。

トップ > python

ROS Pythonでオイラー角とクォータニオンの相互変換

ROS python

ROSでオイラー角とクォータニオンを変換するには、tfパッケージの関数を利用すれば良いのですが、 単体の関数として使うには少々使いづらいので、簡単なラッパー関数を作りました。

オイラー角からクォータニオンへの変換

import tf
from geometry_msgs.msg import Quaternion

def euler_to_quaternion(euler):
    """Convert Euler Angles to Quaternion

    euler: geometry_msgs/Vector3
    quaternion: geometry_msgs/Quaternion
    """
    q = tf.transformations.quaternion_from_euler(euler.x, euler.y, euler.z)
    return Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])

実行例

>>> import math
>>> euler_to_quaternion(Vector3(0.0, 0.0, 0.0))
x: 0.0
y: 0.0
z: 0.0
w: 1.0

>>> euler_to_quarternion(Vector3(0.0, 0.0, math.pi / 2.0))
x: 0.0
y: 0.0
z: 0.707106781187
w: 0.707106781187

クォータニオンからオイラー角への変換

import tf
from geometry_msgs.msg import Vector3

def quaternion_to_euler(quaternion):
    """Convert Quaternion to Euler Angles

    quarternion: geometry_msgs/Quaternion
    euler: geometry_msgs/Vector3
    """
    e = tf.transformations.euler_from_quaternion((quaternion.x, quaternion.y, quaternion.z, quaternion.w))
    return Vector3(x=e[0], y=e[1], z=e[2])

実行例

>>> quaternion_to_euler(Quaternion(0.0, 0.0, 0.0, 1.0))
x: 0.0
y: -0.0
z: 0.0

>>> quaternion_to_euler(Quaternion(0.0, 0.0, 0.7071, 0.7071))
x: 0.0
y: -0.0
z: 1.57079632679

参考URL

ROS Pythonにおけるuint8[]の扱い方

ROS python

ROSでuint8[]型を利用しているトピックをPythonで使う場合にハマった点です。

環境

rospyにおけるuint8[]型の扱い

ROSのメッセージ型に関するリファレンスを参照すると、uint8[]型について以下のような記述があります。

uint8 has special meaning in Python. uint8[] is treated as a Python bytes so that it is compatible with other byte-oriented APIs in Python.

rospy treats uint8[] data as a bytes, which is the Python representation for byte data. In Python 2, this is the same as str.

Pythonでは、ROSのuint8[]型がbytes型(Python 2系ではstr型)として扱われるらしく、これは時に厄介な問題を引き起こします。

発生した問題

とあるセンサー群の測定値を測定するメッセージ型を以下のように定義しました。

foo_package/msg/FooSensor.msg

uint8[] value

そして、以下のようなシンプルなPublisherとSubscriberを作成しました。

publisher.py

import rospy
from foo_package.msg import FooSensor

def publish():
    publisher = rospy.Publisher('/foo', FooSensor, queue_size=1)
    value = FooSensor(value=[100, 110, 120])
    r = rospy.Rate(1.0)
    while not rospy.is_shutdown():
        publisher.publish(value)
        r.sleep()

publish()

subscriber.py

import rospy
from foo_package.msg import FooSensor

def callback(data):
    rospy.loginfo(data)
    rospy.loginfo(data.value)

def subscribe():
    rospy.Subscribe('/foo', FooSensor, callback)
    rospy.spin()

subscribe()

publisher.py, subscriber.pyを実行すると、以下のような2つのINFOレベルのログが出力されます。

  • [INFO] [WallTime: 1461145922.975544] value: [100, 110, 120]
  • [INFO] [WallTime: 1461145941.678116] dnx

data自体を出力すると、valueにint型の数値が入っているのが確認できるのですが、data.valueを出力するとdnxという文字列が出力されてしまい、配列の要素をint型として利用することが出来ません。

対処方法

対処方法はあまり根本的なものではないのですが、valueの各要素のASCIIコードが本来のintの値なので、ord関数で変換してあげることでint型の配列を得ることが出来ます。

def callback(data):
    int_array = [ord(n) for n in data.value]
    rospy.logwarn(int_array)

再度実行すると[100, 110, 120]という配列が出力されます。

ただし、この処理は直感的でなく、uint8[]型以外に対してはエラーを返すため、あまりオススメできません。出来れば、メッセージ型の定義部分でuint16[]等を利用することで回避するのが好ましいと思われます。

参考URL

PIRセンサ(SB00412A-1)とMilkcocoaで簡易見守りシステムを構築

Milkcocoa Raspberry Pi python

見守りシステムで家の中に監視カメラを置くなんて話がありますが、さすがに部屋をずっと撮影されているといい気持ちはしないと思います。そこで、前の記事で紹介したaitendoの人感センサ(SB00412A-1)のデータをRaspberry PiからMilkcocoaにアップロードして、簡単な見守りシステムを構築します。

PIRセンサならば見られてる感もないですし、留守じゃないのに全く反応がなければ何かがおかしいはずなので、簡易的な見守りセンサーとしては役割を果たせるのかもしれません。

f:id:yurayur:20160228212211j:plain

必要なもの

配線

Raspberry Piで人感センサ(SB00412A-1)を5Vで利用する方法 - Yura YuLifeの記事を参考に配線してください。

Pythonからセンサー情報を取得してMilkcocoaへ送信

下準備

Raspberry Pi上でMilkcocoaのPython SDKをインストールしておきます。

$ git clone https://github.com/milk-cocoa/python_sdk.git
$ cd python_sdk/
$ sudo python setup.py install

また、Milkcocoaのウェブサイトでアプリを作成して、API Key とAPI Secretを作成しておきます。

センサー情報をMilkcocoaに送信

以下のスクリプトを保存し、PIRセンサを接続したGPIOのピン番号、MilkcocoaのアプリID、API Key、API Secret、データストア名を記入します。

後は$ nohup ./pir_to_milkcocoa.py &で動かしておけば、30秒毎にMilkcocoaにJSONデータが送信されます。

f:id:yurayur:20160229000612p:plain

アプリの管理画面から、確かにデータが送信されていることが確認できます。

データの可視化

Milkcocoaのfreeboardを利用して、データを可視化します。

画面右上のDatasourcesにデータストアを追加します。DATASTOREにデータストア名を記入、APIはpushを選択します。

f:id:yurayur:20160228214827p:plain

続いてADD PANEからPANEを追加します。TYPEはSparkline、VALUEにはdatasources["データストア名"]["value"]["count"]と記入します。

f:id:yurayur:20160228214658p:plain

下図のように30秒ごとの検知回数がグラフ化されます。

f:id:yurayur:20160229000140p:plain

コレを使えば、不在のはずなのに0以外の値が頻繁に返ってきたり、在宅のはずなのに0ばかりの場合は何かおかしなことが起きている、くらいのことが分かりますね。

参考URL

Raspberry Piで人感センサ(SB00412A-1)を5Vで利用する方法

Raspberry Pi python

aitendoで購入したPIRセンサ(SB00412A-1)ですが、Raspberry Piで3.3Vで利用すると誤検知を繰り返し動作が不安定なので、5Vで利用する方法をまとめました。

f:id:yurayur:20160228234451j:plain

必要なもの

配線

配線は下図のとおりです。

f:id:yurayur:20160228233759p:plain

抵抗3つを利用して、VOUTの5Vの電圧の2/3(=3.3V)を取り出しています。

Raspberry PiのGPIOは5Vに対応していないため、直接VOUTを接続するとRaspberry Pi自体が壊れる恐れがあるので、間違えないよう注意して接続します。

動作用のPythonスクリプトは以前の記事(人感センサ(SB00412A-1)検知時にLEDを光らせる - Yura YuLife)を参照してください。

動作の違い

3.3Vで動作させた場合

人の有無にかかわらず、約7秒間HIGH → 約1秒間LOWを繰り返す。覆いなどをかぶせても同様。

5.0Vで動作させた場合

覆いをかぶせると常にLOWを返し、人が近づいたり動いたりするとHIGHに切り替わる。人が去ると約10秒程度でLOWに切り替わる。

参考URL

ROSのPointCloud2で独自のフィールドを定義

ROS python pcl

ROSのpclパッケージで独自のフィールドを定義したPointCloud2のトピックをpublish, subscribeする方法です。

環境

スクリプト例(Publisher)

rvizで確認

上記のスクリプトを動かし、rvizでPointCloud2型の/custom_point_cloudトピックを表示すると、下図のように生成した3つの点が色付きで表示されることが確認できます。

$ rosrun some_package  publish_custom_point_cloud.py

f:id:yurayur:20160226151803p:plain

フィールドの定義

上記スクリプトではFIELDSの部分で独自のフィールドを定義しています。

FIELDS = [
    # x, y, zやrgbなどはよく使われるフィールド名
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    ...
    # 以下が独自のフィールド
    PointField(name='my_field1', offset=16, datatype=PointField.FLOAT32, count=1),
    ]

よく使われるフィールドについては pcl/Overview - ROS Wiki の 1.3 Common PointCloud2 field names の項目で列挙されています。

PointFieldの各引数は以下のように定義されています。

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

nameとdatatypeは見ての通り、フィールド名とデータ型です。

offsetはPointCloud2の各点を表す構造体の中で、何バイトのところからがそのフィールドを表しているかを定義するようです。 例えばx座標のフィールドのFloat32は4バイトなので、後続するy座標のフィールドはoffset=4となっています。

また、countは当該フィールドのデータ数を表しています。通常は1で良いと思います。

ポイントの生成

POINTS = [
    # FIELDSで定義したフィールドを列挙
    # [x, y, z, rgb, my_field1, my_field2]
    [0.3, 0.0, 0.0, 0xff0000, 0.5, 1.2],
    [0.0, 0.3, 0.0, 0x00ff00, 1.8, 0.0],
    [0.0, 0.0, 0.3, 0x0000ff, 0.9, 0.4],
]

create_cloudの3番目の引数には点のリストであるPOINTSを渡しています。

POINTSの各要素はそれぞれがポイントに対応しており、各ポイントもリスト型になっていて、FIELDSで定義したフィールドの順に値が並んでいます。

例えば、count=1のフィールドが3個の場合は長さ3のリストに、count=1のフィールドが3個とcount=3のフィールドが1個の場合は長さ6にリストになります。

スクリプト例(Subscriber)

このスクリプトを動かすと以下のような出力が表示されます。

$ rosrun some_package  subscribe_custom_point_cloud.py

[WARN] [WallTime: 1456466400.492649] x, y, z: 0.3, 0.0, 0.0
[WARN] [WallTime: 1456466400.492909] my field 1: 0.500000
[WARN] [WallTime: 1456466400.493283] my field 2: 1.200000
[WARN] [WallTime: 1456466400.493446] x, y, z: 0.0, 0.3, 0.0
[WARN] [WallTime: 1456466400.493641] my field 1: 1.800000
[WARN] [WallTime: 1456466400.493788] my field 2: 0.000000
[WARN] [WallTime: 1456466400.493930] x, y, z: 0.0, 0.0, 0.3
[WARN] [WallTime: 1456466400.494073] my field 1: 0.900000
[WARN] [WallTime: 1456466400.494212] my field 2: 0.400000

各ポイントを使うときはread_points関数を利用すると、publisherのPOINTSと同じ形式で点群を取得できます。

参考URL