map_converter

役割

map_converter は rogi_msgs/msg/Map を nav_msgs/msg/OccupancyGrid に rasterize します。emcl2 はこの grid から尤度場を作ります。

入力 map の bounding box を \([x_{\min},x_{\max}]\times[y_{\min},y_{\max}]\) とします。

\[W=\left\lfloor\frac{x_{\max}-x_{\min}}{\Delta}\right\rfloor+1,\qquad H=\left\lfloor\frac{y_{\max}-y_{\min}}{\Delta}\right\rfloor+1\]

world 座標から grid index への変換は次式です。

\[i=\left\lfloor\frac{x-x_{\min}}{\Delta}\right\rfloor,\qquad j=\left\lfloor\frac{y-y_{\min}}{\Delta}\right\rfloor\]

初期 cell 値は 0、占有 cell は 100 です。

grid 上の始点を \((i_0,j_0)\)、終点を \((i_1,j_1)\) とします。

\[d_i=|i_1-i_0|,\quad d_j=|j_1-j_0|,\quad s_i=\operatorname{sign}(i_1-i_0),\quad s_j=\operatorname{sign}(j_1-j_0)\]

Bresenham 型に誤差 \(e=d_i-d_j\) を更新します。

\[2e>-d_j \Rightarrow e\leftarrow e-d_j,\ i\leftarrow i+s_i\]

\[2e<d_i \Rightarrow e\leftarrow e+d_i,\ j\leftarrow j+s_j\]

各 step の \((i,j)\) を 100 にします。start.z と end.z がどちらも 0 でない線分は無視します。

円弧上の点は次式です。

\[\begin{split}p(\theta)= \begin{bmatrix} x_c+r\cos\theta\\ y_c+r\sin\theta \end{bmatrix}\end{split}\]

サンプル数は次式です。

\[N=\max\left(10,\left\lfloor\frac{8r}{\Delta}\right\rfloor\right)\]

\[\theta_k=\theta_s+\frac{k}{N}(\theta_e-\theta_s),\qquad k=0,\dots,N\]

隣接サンプルを Bresenham で結び、さらに cell 中心 \(q=(x,y)\) が次を満たすとき占有にします。

\[\|q-c\|^2\le r^2\]

full circle でない場合は、角度 \(\phi=\operatorname{atan2}(y-y_c,x-x_c)\) が sweep 内にあることも条件です。

\[\operatorname{wrap}_{[0,2\pi)}(\phi-\theta_s)\le \theta_e-\theta_s\]

sample では map_converter 専用 YAML はありません。rogi_launch/launch/components/map.py から次の値だけが渡されます。

設定元	key	対応
`example/sample/config/map/config.yaml`	`topics.raw_map`	`map` subscription の remap 先
`example/sample/config/map/config.yaml`	`topics.map`	`occupancy_grid` publisher の remap 先
`real.yaml` / `sim.yaml`	`launch.components.map.map_converter.enabled`	ノード起動の有効/無効

resolution は launch から渡されていないため、実装 default の \(\Delta=0.01\) [m/cell] が使われます。解像度を profile から変えたい場合は、map.py 側で resolution parameter を渡す必要があります。