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]:何個のデータが入っているか(?)

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

data[]のデータ構造

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の勉強…

コメントを残す

メールアドレスが公開されることはありません。