Nav2で経路はあるのに進めない原因を調べる

経路の線が見えるのにロボットが進まないときは、計画と実行を分けて調べる。経路が存在しても、現在の車体姿勢から実行できる速度指令があるとは限らない。対象はROS 2/Nav2 Jazzy。以下は公式資料を基にした診断手順で、実機走行を検証した記事ではない。

まず同じ停止場面を保存する

停止時のglobal path、local/global costmap、footprint、TF、オドメトリ、速度指令、アクションの結果とログをそろえる。RVizでは同じ時刻・同じ座標で重ねて見る。起動直後と数秒後の画面を比較して原因を断定しない。rosbag2の再生手順を使うと、入力条件をそろえやすい。

図1 · ボタンで表示を切り替え

車体が通る幾何条件を確認する

幅0.60 mの車体を幅0.80 mの通路へ正対させると、中心にある場合の左右の余裕は0.10 mずつである。これは静的な寸法計算にすぎず、位置推定誤差、壁の厚みの表現、積載物、旋回時の張り出しは含まない。長方形は向きが変わると必要な幅も変わる。

footprintは車体外形の多角形、robot_radiusは円による近似に使う。入力した寸法とRViz上の輪郭を比較し、メートル単位、base_linkからの原点、センサや荷物の突き出しを確認する。Nav2のチューニングガイドでは車体形状とコスト場を扱っている。停止を消すために実物より小さい輪郭へ変更してはいけない。

コストは物理的な壁と同じではない

障害物レイヤーは観測を地図へ反映し、inflationは障害物周辺に距離に応じたコストを広げる。inflation_radiusはその広がり、cost_scaling_factorは減衰の速さに関係する。ほかの条件が同じなら、scalingを大きくすると距離による指数減衰が速くなる。Inflation Layerの説明を参照する。

高コストの帯をすべて「絶対に入れない領域」と読むのも、inflation_radiusをそのまま車体の最低離隔距離と読むのも正確ではない。衝突判定、車体外形、各planner/controllerのコスト利用を合わせて考える。global mapは通れてもlocal mapに古い障害物が残るなら、センサのclearing、観測範囲、TFの有効時刻を先に確認する。

経路から実行可能な動きへ

経路は通る場所を示す。controllerは現在速度や運動制約を考えて次の指令を作る。狭い場所で大きく向きを変える、最小旋回半径がある車体にその場回転を求める、加速度制限で候補が実行できない、といった不一致が停止につながり得る。

Controller Serverの資料で、選択中のcontroller・progress checker・goal checkerを確認する。progress checkerは進捗、goal checkerは到達を判定する仕組みであり、単に同じ「止まる設定」ではない。プラグイン名とエラーメッセージを記録してから、そのプラグインの制約を読む。

指令がどこで消えるかを追う

/cmd_velという名前だけで最終段と決めつけない。構成によって中間topicがあり、TwistとTwistStampedも異なる。ros2 topic info <実際のtopic名> --verboseで配信元・購読先・型を追う。停止機能を解除して診断するのではなく、どの条件が成立して停止したかをログで確認する。

変更を一つずつ判定する

通路、開始姿勢、目標、地図、入力ログを固定し、一項目だけ変更する。到達の有無に加えて、最小離隔、停止回数、指令、失敗理由を比較する。本記事の0.10 mという計算から安全な余裕値を決めることはできない。実機の寸法と誤差、利用環境に基づく別の確認が必要である。

起動そのものが不安定なら、先にNav2の最小構成へ戻る。構成が安定してから診断を始めると、地図の問題とドライバの問題を混ぜずに説明できる。