kiks_avoidance #927
Draft
kiks_avoidance #927
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
CLAUDE.mdから共通部分をAGENTS.mdへ移植 - CLAUDE.mdはClaude固有補足のみ残しAGENTS.md参照 - フッター要件を両ファイルから削除 - チェックリストの該当項目も整理
- GraphPlannerを追加（式(1)/(2)対応、距離/時間コスト切替）
- 動的障害物モデルとオンデマンドグラフ拡張（接線生成）
- ウェイポイント到達速度をRVOのPreferred Velocityへ反映
- 可視化強化（障害物/始終点/経路/速度ベクトル）
- CMake更新（graph_plannerのビルド追加）
パラメータ:
- use_graph_planner（既定true）
- graph_planner.use_time_cost（時間ベース切替）
- graph_planner.{static_margin,K_alpha,K_v,K_t,far_margin_cap} 等
- Boxの膨張関数 inflateBox を追加 - ポリライン距離計算 polylineLength を追加 - 始端/終端速度対応の getReachTime を追加（台形/三角プロファイル）
- CircleObstacle/BoxObstacleをCircle/Boxの別名に変更 - 交差判定をcrane_geometry/Boost.Geometryのヘルパで統一 - ペナルティエリア膨張にinflateBoxを使用 - コメントを日本語化し簡潔化
- getSegmentTime: L, v_in, v_end, acc/dec, vmaxから時間を算出 - getTravelTimeTrapezoidal: v_end引数を追加（デフォルト0.0）し内部で共通計算を使用 - 旧4引数版の実装は廃止（後方互換はデフォルト引数で担保）
- getTravelTimeTrapezoidal(robot, target, acc, vel, v_end=0.0) に置換 - 進行方向速度の手計算を廃止しシンプル化
- getTravelTimeTrapezoidal(robot, point, 0.5, 2.0, 0.0)を使用 - 到達時間の算出を1行に簡略化
- 箇条書きの行分割を調整し可読性を改善
- long long を std::int64_t/std::uint64_t に置換 - ビット演算の即値に ULL サフィックスを付与 - Cスタイルキャストを static_cast に置換
- goalie/world_model_wrapper/travel_time の長行を折返し - 可読性向上のための整形のみ（動作影響なし）
- 構築時の検証: 空配列(未設定)は例外にしないが、厳密な不整合は例外 - 予測関数: 軽微な範囲外(±0.15)はクランプし、大きな外れは例外 - これにより InvalidConfig/PredictStraightKickSpeed/ValidateConfig テスト要件に適合
- 一時オブジェクト生成をブロックで明示してコンストラクタ例外を捕捉
- 始端/終端速度・加減速度・最大速度を考慮する6引数版を追加 - graph_plannerの時間コスト評価で使用可能に
- 片側のみ空や配列サイズ不一致を明示的に例外化 - テストInvalidConfigに整合
- CostMode と use_time_cost パラメータを削除 - 距離コスト分岐をコメントアウトし、時間コスト評価をデフォルト採用
- USE_GRAPH_PLANNER と関連ROSパラメータを削除 - 例外をcatchしてフォールバックする挙動は維持
- getTravelTime(単純版)を削除 - getTravelTimeTrapezoidal(robot, target, acc, vel, v_end=0.0)に一本化 - READMEからSimple Travel Timeの記述を削除
- plan() 内の可視化ブロックの最後で viz_->flush() を呼び出し - 即時にSVGレイヤへ反映されるように修正
- path_pts を polyline().addPoint(...) で一括描画 - 表示は従来同等（シアン色、太さ維持）
- コンストラクタを (node, world_model) に変更 - RVO2Planner側の生成呼び出しを更新 - 内部でレイヤ 'graph_planner' の VisualizerMessageBuilder を生成
- 最近傍WPの目標速度が0の場合、進行方向（次点 or 目標）から速度を再構成 - terminal_velocityの下限を0速度にも適用できるよう方向復元して補正 - これにより経路終端付近でも優先速度が0になりにくくする
- 加速度ゼロ/距離極小時でも下限速度(terminal_velocity)を適用 - STOP以外では min-cap の後に下限をmax()で反映 - 数値安定化: decel計算でmax(0, ...) による sqrt 保護
- RVO2Planner側のworld_modelがnullptrになる回収を防止 - コンストラクタでstd::moveを外して参照共有に変更
- 次点target_velocityが0のとき、v_suggestにlimits.vmaxを用いて非ゼロ速度を構成 - terminal_velocityが0でも各フレームの上限内で移動を開始可能に
- world_がnullなら2点経路で早期返却 - states参照前にサイズを保証し、不整合時に-1終端 - viz_存在時のみ描画・flushを実施
- GraphPlanner: 入出力要約、expansions/nodes/goal_node、フォールバックやstates不整合をログ - RVO2: max_velが0に潰れた際やpref=0の際にWARNを出力、例外時はERROR
- world_==null フォールバック削除 - viz_存在チェックを外し常に描画/flush（GraphPlannerが内部生成）
- GraphPlanner::planからflush()を削除 - GraphPlannerにpublic flush()追加 - RVO2PlannerのreflectWorldToRVOSim末尾でflush()を一回だけ呼ぶ
- use_time_cost/CostMode関連を削除しpolyline距離のみで評価 - 円障害物の統合ロジックを廃止し、各ロボット円をそのまま使用 - 近接ノードのスナップ(node_merge_epsilon)は維持
- ゴール→円の接線接点を用いて過剰な円周追従を抑制 - 他障害物と交差しない場合に限り接線接続を採用 - 可視化・ウェイポイント生成はスムージング後の点列で実施
従来の複雑な経路探索ロジックを、疑似コードに準拠したより効率的で 理解しやすいアルゴリズムに全面的に書き直し。 主な変更: - buildObstacles(): ロボットIDベースの簡潔な障害物生成に変更 - plan(): Dijkstraアルゴリズムの実装を疑似コード準拠に改良 - 新規補助関数群を追加: - firstIntersection(): 最初の交差障害物検出 - expandUntilLineOfSight(): 再帰的候補展開 - boxCornersOutward(): 矩形障害物角点処理 - getOrCreateNodeAt(): ノード管理の効率化 技術的改善: - Open/Closed集合による正確なDijkstra実装 - 交差判定の精密化とパフォーマンス向上 - メモリ使用量の最適化（動的リサイズ対応） - デバッグログの改善とエラーハンドリング強化 動作への影響: - より短く滑らかな経路生成が期待される - 計算時間の短縮と安定性の向上 - 複雑な障害物環境でのロバスト性向上 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <[email protected]>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
No description provided.