ROS2のrosbagをROS1に変換しVLP-16のデータをpcapで抽出する

# はじめに
[技術チャレンジ部](https://challenge-club.connpass.com/)の、とも(Tomo)ことAirです。
[自動運転AIチャレンジ](https://www.jsae.or.jp/jaaic/)に参加しているメンバーを応援するために、ROS2のrosbagをROS1に変換しVLP-16のデータをpcapで抽出する環境を構築しました。

# 自動運転AIチャレンジにおけるrosbag
[自動運転AIチャレンジ2023インテグレーション大会](https://github.com/AutomotiveAIChallenge/aichallenge2023-integ)では、自動運転車が霧を切り抜ける(あっ)という[課題](https://www.jsae.or.jp/jaaic/2023ver/contest/)が設定されました。この霧はLiDARで検知されることが想定されており、実車から取得したrosbagデータが運営の方より共有されました。

LiDARは[VelodyneのVLP-16](https://velodynelidar.com/products/puck/)が用いられるようです。VLP-16にはEthernetが付いており、データはUDPで送信されます。これをtcpdumpやWiresharkなどで取得し、pcap形式で保存するということが一般に行われています。[VeloView](https://www.paraview.org/veloview/)というビューアでも、入力ファイルとしてpcap形式が用いられています。

よって、rosbagからpcapに変換できれば、解析ツールの幅が広がるのでは(≒VeloViewのサイバーな見た目がかっくいいので気分があがった結果もしや優勝できてしまうのでは)、という仮説から始めたら、意外と深かった…という奮闘記的ポエムです。

技術チャレンジ部メンバーによる[こちらの記事](https://qiita.com/kiwsdiv/items/cf92182450b2cde7b921)にある、

> チームメンバーにVLP16のデータをrosbagからpcapに変換していただき

の1行に詰まった物語です。

# pcapへの変換方針
技術チャレンジ部メンバーによる[こちらの記事](https://qiita.com/bushio/items/ce0b8fdf42004bada01b)で、rosbagデータの再生方法が具体的に示されています。そこで、大きく2種類のアプローチを考えました。

- rosbagをROSで再生して、トピックとして受信後に、pcap形式に変換する
- rosbagファイルを直接pcapファイルに変換する

直接pcapに変換できれば、ROSの環境も不要で静的に変換できるかと思い(フラグ)、直接pcapに変換する方法を優先的に調査しました。

# rosbagファイルをpcapに変換する方法
具体化するにあたり、再度大きく2種類のアプローチを考えました。

- 変換ソフトを自作する
- 既存の変換ソフトを使用する

どちらにせよ、ChatGPTに聞いてその通りやれば、どうにかなるだろうという軽い気持ちで始めました(フラグ)。
色々試行錯誤しながら調べて得られた中間的な結論は、

- ChatGPTの言う通りにやってもうまく行かない
    - 主にROS1の知識を前提としていそう(自分のプロンプトにも問題あり)
- ROSにはROS1とROS2があり、rosbagの形式も異なる
    - [本大会はROS2で行われている](https://automotiveaichallenge.github.io/aichallenge2023-integ/setup/index.html)ため、rosbagもROS2形式
- [bag_to_pcap](https://github.com/velodynevls/velodyne_vls/tree/master/bag_to_pcap)という、そのものずばりなソフトがある
    - …が、最終更新が5年前と古く、どうもROS1を前提としている
- [rosbags](https://gitlab.com/ternaris/rosbags)というソフトで、ROS2のrosbagをROS1に変換できそう

さあ、考えどころです。これらを合わせると、rosbagsでROS1のrosbagに変換した後、bag_to_pcapでpcapに変換するというアプローチが浮かんできます。

# rosbagsによるROS2からROS1への変換

[rosbagsの本家](https://gitlab.com/ternaris/rosbags)を見ると、

> - rosbag2 reader and writer,
> - rosbag1 reader and writer,
> (中略)
> - efficient converter between rosbag1 and rosbag2,

とあるので、ROS2からROS1への変換はできると信じます。
…が、世の中、ROS1からROS2への変換情報ばかりで、不安になります。

それでも、rosbagsを`pip install rosbags`でインストールし、LiDARのトピック名を調べた上で、`rosbag-convert`を実行すると、エラーとなります。

```shell-session
$ rosbags-convert --include-topic /sensing/lidar/top/velodyne_packets smoke1
ERROR: Converting rosbag: TypesysError("Type 'velodyne_msgs/msg/VelodyneScan' is unknown.")
```

変換に必要な型が定義されていないようです。
[rosbagsのtypes.py](https://gitlab.com/ternaris/rosbags/-/blob/master/src/rosbags/typesys/types.py?ref_type=heads)を見ると、様々な具体的な型が定義されていますが、Velodyneに関するものは無さそうです。しかも、

```python
# THIS FILE IS GENERATED, DO NOT EDIT
```
などとあり、自動生成であることが分かります。

## Velodyne関連型の定義

ROS本家の情報を確認すると、Velodyneに関する型の定義情報があります。
VelodynePacketとVelodyneScanの2種類があるようです。

- VelodynePacket
    - https://docs.ros.org/en/noetic/api/velodyne_msgs/html/msg/VelodynePacket.html
    - https://github.com/ros-drivers/velodyne/blob/master/velodyne_msgs/msg/VelodynePacket.msg
- VelodyneScan
    - https://docs.ros.org/en/noetic/api/velodyne_msgs/html/msg/VelodyneScan.html
    - https://github.com/ros-drivers/velodyne/blob/master/velodyne_msgs/msg/VelodyneScan.msg

この2つの型を、rosbagsのtypes.pyで定義する必要がありそうです。

types.pyにある他の型を真似しながら、手動で記述してみたものの、うまく行きません。
特に、VelodynePacketは、配列が用いられています。[VelodynePacket.msg](https://github.com/ros-drivers/velodyne/blob/master/velodyne_msgs/msg/VelodynePacket.msg)の1206バイトをどのように表現するか、自信が持てませんでした。

## rosbagsの型定義コード生成

**注意: この章は、rosbagsの正しい使い方を知らないド素人がてきとーにhackしたポエムです**

[rosbagsのマニュアル](https://ternaris.gitlab.io/rosbags/examples/register_types.html)を見ると、ROSで定義する型表記を、文字列として記述した上で、`register_types()`で登録することができるようです。

そこで、こんなコードを書いてみます。

```python
PACKET_MSG = """
time stamp
uint8[1206] data
"""
SCAN_MSG = """
Header           header
VelodynePacket[] packets
"""
register_types(get_types_from_msg(PACKET_MSG, 'velodyne_msgs/msg/VelodynePacket'))
register_types(get_types_from_msg(SCAN_MSG, 'velodyne_msgs/msg/VelodyneScan'))
```

すると、なんと、rosbagsが[register.py](https://gitlab.com/ternaris/rosbags/-/blob/master/src/rosbags/typesys/register.py?ref_type=heads)の中で、型定義のPythonコードを生成するようです。すごっ！

そこで、コード生成関数である`generate_python_code()`がreturnする直前で、`print(lines)`するように改変し、内容を確認します。

ここで得られた内容をtypes.pyに反映したパッチが[こちら](https://github.com/rpng/rosbags/compare/master...air-gh:rosbags:velodyne-dev)。
(rosbagsのバージョンにより微妙に表記が異なる可能性があります)

このtypes.pyを組み込んだrosbagsで、ROS2のrosbagをROS1に変換することができました。

# bag_to_pcapによるrosbagからpcapへの変換

あとはbag_to_pcapを実行するだけ！もう8合目越えた！(フラグ)
…な気分で実行するものの、色々と問題が起こります。
試行錯誤の結果、以下のような結論が得られました。

- bag_to_pcapはPython2
    - rosbagsをpipで入れた時も、roslibpyをPython2で入れる(?)ので、Python2がちら見えしていた…
- ビルドにROS1が必要
- 実行にもROS1が必要
    - 静的な変換ではなく、ROS1でrosbagを再生しながら、bag_to_pcapで変換するっぽい

この時点でフラグを回収しまくりです。前進あるのみで検討した結果、

- Python3フリーなROS1環境を作る

ことにします。具体的には、Ubuntu 16.04まで戻りROS1環境を作成する、という前進です。

## velodyne_vlsのビルド

**注意: 以下はUbuntu16.04上のROS1環境です**

bag_to_pcapは、[velodyne_vls](https://github.com/velodynevls/velodyne_vls)に含まれています。
まず、依存パッケージをインストールします。

```shell-session
pip install roslibpy
sudo apt install libpcap-dev
```

次に、velodyne_vlsをビルドします([ROS本家マニュアル](https://wiki.ros.org/ROS/Tutorials/CreatingPackage))。

```shell-session
mkdir src
cd src
git clone https://github.com/velodynevls/velodyne_vls.git
cd ..
catkin_make -DCMAKE_BUILD=Release
```

## bag_to_pcapの実行

まず、roscoreを実行します([ROS本家マニュアル](https://wiki.ros.org/ja/roscore))。

```shell-session
roscore -p 11311
```

次に、ROS1環境の環境変数を読み込みます([ROS本家マニュアル](https://wiki.ros.org/ja/ROS/Tutorials/InstallingandConfiguringROSEnvironment))。

```shell-session
source devel/setup.bash
```

bag_to_pcapを実行します。

```shell-session
rosrun bag_to_pcap bag_to_pcap.py /sensing/lidar/top/velodyne_packets
```

rosbagを再生します。

```shell-session
rosbag play smoke1.bag
```

ついにpcapが得られました！

# VeloViewによるpcapの再生
pcapファイルできたんだからもう10合目だろう！(フラグ)
VeloViewで再生すると、再生が一瞬で終わってしまいます。当初、これが期待動作なのか分からず…。
Wiresharkでpcapを見ると、タイムスタンプが絶対時刻で2018年2月で同じ、相対時刻で見ると0です。
[bag_to_pcapのソース](https://github.com/velodynevls/velodyne_vls/blob/master/bag_to_pcap/scripts/bag_to_pcap.py)を見ると、仕様のように思えます。

```python
#Global header for pcap 2.4
global_header =  'd4c3b2a10200040000000000000000000000010001000000'         # 24 bytes = 48 characters.
```

[VLP-16のマニュアル](https://velodynelidar.com/wp-content/uploads/2019/12/63-9243-Rev-E-VLP-16-User-Manual.pdf)を見ると、パケット構造が詳細に説明されており、UDPのペイロードとして、LiDARのデータとともにタイムスタンプも入っていることが分かります。
ついでに、Factory Byteから、Return ModeがStrongest(Dual ReturnではなくSingle Return)で、Product IDがVLP-16であることも分かりました。

pcapのタイムスタンプを、
- VLP-16のペイロードのタイムスタンプから生成する
- rosrun bag_to_pcap実行時のパケット取得タイミングからタイムスタンプを生成する

といった案を出している最中に、VeloView 3.5.0だと再生できるという情報が入ってきます。
自分は最新のVeloView 5.1を入れていました。

ということで、[過去バージョンもあるダウンロードページ](https://www.paraview.org/veloview/#download)から、3.5.0をダウンロードして再生します。
個人的に感動の再生結果が[こちらの動画](https://www.youtube.com/watch?v=AzYKJP3ZNs4)！

https://youtu.be/AzYKJP3ZNs4

# 感想いろいろ
- 完成度
    - きっともっと良い最短ルートがある
    - VeloViewの再生結果を見ると、変換が不完全な可能性がある
- 改善点
    - rosbagsは正しい方法で型定義しよう
    - bag_to_pcapをPython3化しよう
    - bag_to_pcapをROS2化しよう
    - ROS2のrosbagから直接pcap化するソフトを書こう
    - pcapにタイムスタンプを入れよう
    - pcapからLiDARデータをROSに流すUDP再生ソフトとかどうかしら
- やってみて
    - フラグは回収される
    - それでも、実際に手を動かすと、得られることがたくさんある
    - 一緒にチャレンジする方、結果を活用する方、みなさんに元気をいただきました！

# 謝辞
この記事の範囲だけでも、ここにたどりつくまでに、たくさんの方から、たくさんのことを教えていただきました。
[技術チャレンジ部](https://challenge-club.connpass.com/)のみなさま、本当にありがとうございました！

- @bushio さん
    - ROSとrosbagの基礎知識
- @kiwsdiv さん
    - LiDARとVLP-16の基礎知識
    - VeloViewの使い方
- マレインさん
    - rosbag変換のはまりどころ
    - ROS1の実行環境
- @seigot さん
    - 高い、でもあきらめない、目標