ROSのPointCloudで点群に色を付ける

ROSのrvizで色付き点群を表示しようとした時に,PointCloud型のメッセージで色情報を付与する際にハマったので,メモしておきます.

 

 

sensor_msgs::PointCloud

ROSでPointCloudを扱うときには,sensor_msgs::PointCloudというメッセージ型を使用します.
PointCloud型は点群の座標(x,y,z)情報に加えて,スレレオカメラで得られる色情報や,lidarから得られるレーザの反射強度intensity情報を付与することができます.
ROSのPointCloud型Messageのドキュメントを見てみましょう.

sensor_msgs/PointCloud Message

PointCloud型は,データの座標系やタイムスタンプ等を格納するheader,点群の座標の配列を格納するpoints,点の色や反射強度等の付属の情報の配列を格納するchannelsという要素から成り立っています.
このchannelsに色情報を格納することで色付き点群としてrviz上に表示することができます.
ChannelFloat32型について詳しく見ていきましょう.

 

 

sensor_msgs/ChannelFloat32 Message

ChannelFloat32型はPointCloudの点群に色々な情報を付与でき,予め指定されたフォーマットに従うと,rviz上でもそれにならってビジュアライズしてくれます.

sensor_msgs/PointCloud Message

ChannelFloat32型のメッセージはそのデータが何であるかを示すnameと,その情報を格納した配列であるfloat32型の配列であるvaluesからなります.
ここでnameに”rgb”を指定して,valuesに正しい値を入れてあげることで点群をカラー表示することができます.
“rgb”の部分の説明を見ると,unsigned_int8型で記述された値を(R,G,B)の順番で下位24ビットに順番に入れよ.となっています.が,
valuesの型は32ビットfloatとなっています.

 

 

解決策

reintrepret_castを使って,uint32型を強制的にfloat型にキャストすることで解決しました.
以下にサンプルコード載せておきます.

channelsの情報でintensity等の浮動小数の値も格納したかったためにfloat型を採用したのだとは思いますが,そこに整数型の値を突っ込むわけですから,混乱を招くわけです...
まあ,解決出来たので良しとしましょう.