ROSのPointCloud2で独自のフィールドを定義
ROSのpclパッケージで独自のフィールドを定義したPointCloud2のトピックをpublish, subscribeする方法です。
環境
- Ubuntu 14.04
- ROS Indigo
スクリプト例(Publisher)
rvizで確認
上記のスクリプトを動かし、rvizでPointCloud2型の/custom_point_cloudトピックを表示すると、下図のように生成した3つの点が色付きで表示されることが確認できます。
$ rosrun some_package publish_custom_point_cloud.py
フィールドの定義
上記スクリプトでは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と同じ形式で点群を取得できます。
