scan_merger

役割

scan_merger は複数の LaserScan を 1 本の仮想 scan に再投影します。入力は各 LiDAR frame、出力は frame_id parameter、通常 base_link です。

Interface

種別	名前	型 / 内容
subscribe	`scan_topics`	`sensor_msgs/msg/LaserScan` の配列
publish	`merged_scan` remap 後 `/scan_for_localization`	`sensor_msgs/msg/LaserScan`
parameter	`frame_id`	出力 frame
parameter	`frequency`	publish 周期
parameter	`angle_min`, `angle_max`, `angle_increment`	出力 scan の角度 grid
parameter	`range_min`, `range_max`	出力 scan の距離範囲

出力 bin

出力 bin 数は次式です。

\[N=\left\lfloor\frac{\theta_{\max}-\theta_{\min}}{\Delta\theta}\right\rfloor+1\]

初期 range は全 bin で \(+\infty\) です。

\[r_k=+\infty,\qquad k=0,\dots,N-1\]

入力 beam の再投影

入力 beam index \(i\) の角度は、

\[\theta_i^S=\theta_{\min}^S+i\Delta\theta^S\]

です。有効 range \(\rho_i\) を入力 frame の点へ変換します。

\[\begin{split}p_i^S= \begin{bmatrix} \rho_i\cos\theta_i^S\\ \rho_i\sin\theta_i^S\\ 0 \end{bmatrix}\end{split}\]

TF で出力 frame へ変換します。

\[p_i^O=T^O_S p_i^S\]

出力 frame で極座標に戻します。

\[\rho_i^O=\sqrt{(x_i^O)^2+(y_i^O)^2},\qquad \theta_i^O=\operatorname{atan2}(y_i^O,x_i^O)\]

出力範囲に入る場合、bin index は丸めで決めます。

\[k=\operatorname{round}\left(\frac{\theta_i^O-\theta_{\min}}{\Delta\theta}\right)\]

同じ bin では最小距離を採用します。

\[r_k\leftarrow\min(r_k,\rho_i^O)\]

この最小化により、複数 LiDAR が同じ方向を見た場合も LaserScan として自然な「最初の hit」を残します。

時刻ずれ

厳密な同期は行いません。topic ごとの最新 scan を timer 周期で統合します。ロボット速度を \(v\)、scan 間の時刻ずれを \(\Delta t\) とすると、並進方向の見かけ誤差は概ね \(v\Delta t\) です。

設定ファイル

sample では example/sample/config/sensing/config.yaml の scan_merger.ros__parameters と sick_picoscan の両方が関係します。

設定元	key	対応
`sensing/config.yaml`	`scan_merger.ros__parameters.scan_topics`	picoscan 無効時の入力 scan topic 配列
`sensing/config.yaml`	`sick_picoscan.lidars[].publish_laserscan_fullframe_topic`	picoscan 有効時の入力 scan topic 配列
`localization/config.yaml`	`topics.localization_scan`	`merged_scan` の remap 先
`real.yaml` / `sim.yaml`	`launch.components.sensing.scan_merger.enabled`	ノード起動の有効/無効

picoscan が有効な場合、scan_topics は YAML の scan_merger ではなく LiDAR 定義から自動生成されます。

\[S=[\mathrm{lidars}[i].\mathrm{publish\_laserscan\_fullframe\_topic}]\]

picoscan が無効な場合は scan_merger.ros__parameters.scan_topics を使います。sample では次です。

scan_merger:
  ros__parameters:
    scan_topics:
      - /lidar1/scan
      - /lidar2/scan

frame_id と frequency は YAML ではなく sensing.launch.py の launch argument です。default は base_link と 40.0 Hz です。angle_min/max、angle_increment、range_min/max、scan_time は launch から渡されないため、実装 default が使われます。