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
と同じ形式で点群を取得できます。