Yura YuLife

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

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

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 Wiki1.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

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

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