map_loader

役割

map_loader は CSV 地図を rogi_msgs/msg/Map に変換して publish します。線分と円弧を 1 つの map message にまとめ、map_converter、ransac_localizer、可視化ノードへ渡します。

線分の始点と終点を \(p_s=(x_s,y_s,z_s)\)、\(p_e=(x_e,y_e,z_e)\) とします。reverse_y=false ならそのまま publish します。reverse_y=true の場合、y 座標だけを次式で反転します。

\[y' = -y + y_0\]

ここで \(y_0\) は reverse_y_offset です。

\[p'_s=(x_s,-y_s+y_0,z_s), \qquad p'_e=(x_e,-y_e+y_0,z_e)\]

円弧は中心 \(c=(x_c,y_c,z_c)\)、半径 \(r\)、開始角 \(\theta_s\)、終了角 \(\theta_e\) として読みます。y 軸反転時は次式です。

\[c'=(x_c,-y_c+y_0,z_c), \qquad \theta'_s=-\theta_s, \qquad \theta'_e=-\theta_e\]

publisher は transient_local + reliable です。起動直後に publish し、その後 500 ms 間隔で数回再送します。静的地図を想定しているため、再送後は timer を停止します。

sample では example/sample/config/map/config.yaml がこのノードの入力ファイルを指定します。

key	例	対応する parameter / 用途
`map.line_segments_path`	`line_segments.csv`	`line_segments_path` に渡される線分 CSV
`map.circles_path`	`circles.csv`	`circles_path` に渡される円弧 CSV
`topics.raw_map`	`/raw_map`	`map` publisher の remap 先
`topics.map`	`/map`	後段 `map_converter` の出力 topic

rogi_nav.launch.py は config_dir/map を基準に相対 path を絶対 path へ変換します。

\[P_{\mathrm{line}}=\operatorname{join}(D_{\mathrm{map}},\mathrm{line\_segments\_path})\]

\[P_{\mathrm{circle}}=\operatorname{join}(D_{\mathrm{map}},\mathrm{circles\_path})\]

line_segments.csv は header 後に次の 6 列を持ちます。

start_x,start_y,start_z,end_x,end_y,end_z

circles.csv は次の 6 列です。

center_x,center_y,center_z,radius,start_angle,end_angle

profile 側では real.yaml / sim.yaml の次の key で起動を切り替えます。

launch:
  components:
    map:
      map_loader:
        enabled: true