ROSのsensor_msgs:PointCloud2データの構造と扱い方

はじめに

研究でデプスセンサを使うことがよくあるのですが、デプスセンサからの送られてくるsensor_msgs::PointCloud2形式のデータの扱いについて困った事がありました。例えばあるxy座標でのz座標を取り出したり、ある範囲のデータだけ取り出したりするのが意外と難しかったので、つまずいたことをまとめておきます。

データ形式

ROSの公式ドキュメントを見ればデータ形式そのものはわかるのですが、意味を理解するのにちょっと時間がかかりました。

まず、ポイントクラウドのデータ本体は「data」にあります。これは8ビット(0~255)の数字の列になっていますが、このデータの読み方を定義しているのが「fields」です。ROSのトピックを見てみると、次のようなデータになっていることがわかります。

rostopic echo /point_cloud/fields
- 
  name: "x"
  offset: 0
  datatype: 7
  count: 1
- 
  name: "y"
  offset: 4
  datatype: 7
  count: 1
- 
  name: "z"
  offset: 8
  datatype: 7
  count: 1
--

fieldsは配列になっていて、各データの見方を表しています。
各fieldの見方について説明すると、

• [name]：値の名前
• [offset]：先頭から何番目に始まっているか
• [datatype]：データ型
• [count]：何個のデータが入っているか（？）

を表します。つまりデータの入れ方は次のようになります。

point_stepは1つの点を何個のデータで表すか、で今回は12となります。
さて、xyz各データの読み方ですが、これはfieldのdatatypeで読むことになります。ドキュメントによると今回のデータ型「7」はfloat32を意味するとのことなので、この4バイトをfloat型で読み込めばいいということになります。(4バイトをfloatにする方法はネットに資料が沢山ありますので割愛します。)

data[]とfields[]以外のデータは主にポイントクラウドの大きさなどを表す情報で、

• [height]：高さ
• [width]：幅
• [is_bigendian]：datatypeがビッグエンディアンかどうか
• [point_step]：1つの点を何個のデータで表すか
• [row_step]：1行を何個のデータで表すか
• [is_dense]：すべての値が正常かどうか

などがあります。

ROSの勉強…