の続き
やりたいこと
rosbagsを用いてトピックを読み出すことには成功した
IMUなどのデータはそのままCSVにでもすればよいが，LiDAR点群はそうもいかないのでなんとかしたい．
具体的にはPointCloud2型のトピックになっている点群データをPCD形式のファイルで保存できるようにする．
PointCloud2型の解釈
rosbagsで読んできたPointCloud2型のトピックがこんな感じ
> vars(msg)
{'header': std_msgs__msg__Header(stamp=builtin_interfaces__msg__Time(sec=1713513002, nanosec=460340972, __msgtype__='builtin_interfaces/msg/Time'), frame_id='lidar', __msgtype__='std_msgs/msg/Header'),
'height': 1,
'width': 346779,
'fields': [sensor_msgs__msg__PointField(name='x', offset=0, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
sensor_msgs__msg__PointField(name='y', offset=4, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
sensor_msgs__msg__PointField(name='z', offset=8, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
sensor_msgs__msg__PointField(name='intensity', offset=16, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField')],
'is_bigendian': False,
'point_step': 32,
'row_step': 11096928,
'data': array([245, 8, 132, ..., 1, 116, 255], dtype=uint8),
'is_dense': True,
'__msgtype__': 'sensor_msgs/msg/PointCloud2'}
msg.dataにデータが入っているが，uint8型のクソ長1次元配列になっていて読み方がよくわからない．
読み方が書いてあるのがsensor_msgs__msg__PointFieldのあたりで，この場合
- point_stepが32なので，uint8の配列の32要素でひとかたまり（1つの点）である．
- datatype=7なので型はFLOAT32（uint8の配列4要素で1つの値）．
- [0]～[3]の値がx座標，[4]～[7]の値がy座標，[8]～[11]の値がz座標，[16]～[19]の値が信号強度を表している
ということがわかる．
（参考文献 https://ppdr.softether.net/ros-pointcloud2-data ）
実際適当にreshapeしてやるとそれっぽい感じデータが入っているように見える（[12:16]にも何か値が入っているが謎）
> msg.data[:64].reshape(-1,4)
array([[245, 8, 132, 61],
[115, 181, 179, 64],
[ 10, 92, 23, 63],
[ 0, 0, 128, 63],
[ 0, 0, 184, 65],
[ 0, 0, 0, 0],
[ 0, 0, 0, 0],
[ 0, 0, 0, 0],
[223, 149, 174, 190],
[112, 27, 219, 63],
[136, 184, 222, 63],
[ 0, 0, 128, 63],
[ 0, 0, 240, 65],
[ 1, 0, 0, 0],
[ 0, 0, 0, 0],
[ 0, 0, 0, 0]], dtype=uint8)
あとはuint8の配列4要素（4バイト）分をfloat32として解釈してやれば意味のある値が得られる．
そのためにはtobytes()とnp.frombuffer()を用いる．
具体的にはこう
np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)
> np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)[:3]
array([[ 6.4470209e-02, 5.6158996e+00, 5.9124815e-01, 1.0000000e+00,
2.3000000e+01, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00],
[-3.4098718e-01, 1.7117748e+00, 1.7400064e+00, 1.0000000e+00,
3.0000000e+01, 1.4012985e-45, 0.0000000e+00, 0.0000000e+00],
[ 1.3993801e-01, 6.2663093e+00, 6.5944964e-01, 1.0000000e+00,
1.3000000e+01, 2.8025969e-45, 0.0000000e+00, 0.0000000e+00]],
dtype=float32)
ここで例えば_[:,0]とかすればx座標列が得られる．
PCDファイルの書き方
実態はただのテキストファイルなので，お作法に従ってテキスト出力すればよい．
今回はとりあえず信号強度は気にせずにx,y,z座標だけ書き込むことにする．
書式は以下のような感じで，
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 346779
HEIGHT 1
POINTS 346779
DATA ascii
0.06447021 5.6158996 0.59124815
-0.34098718 1.7117748 1.7400064
(以下略)
ヘッダーに必要事項を書いた後，スペース区切りでx,y,zの値を1行ずつひたすら書いていけばよい．
ヘッダーのうちWIDTHとPOINTSのところには点の数を記入する．msg.widthの値を読んで入れてもいいし，respaheした配列の長さを調べてもいい．
結論
以下のスクリプトでタイムスタンプをファイル名としたPCDファイルが（大量に）出力される．
ファイルサイズが馬鹿にならないので，実用上はまとめて圧縮するなり，必要なファイルだけ逐一rosbagから変換するなり考える必要がある．
src=Path('ファイルパス')
with Reader(src) as reader:
connections = [x for x in reader.connections if x.topic == トピック名]
for connection, timestamp, rawdata in reader.messages(connections=connections):
msg = deserialize_cdr(rawdata, connection.msgtype)
p=np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)
with open("str(timestamp)+".pcd","wt") as f:
f.write("VERSION 0.7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\nCOUNT 1 1 1\nWIDTH "+str(len(p))+"\nHEIGHT 1\nPOINTS "+str(len(p))+"\nDATA ascii\n")
for i in range(len(p)):
f.write(" ".join(np.array(p[i,:3],dtype=str))+"\n")