fix(goal_planner): failed to generate candidate paths when entering another lane for avoidance

[kosuke55]

Cherry-picked from autowarefoundation#11560

avoidanceが発動中にgoal plannerの経路が出ないケースに対する対応。

• https://tier4.atlassian.net/browse/T4DEV-34834
• https://tier4.atlassian.net/browse/T4DEV-38876
  における一部のケースにおいて停車可能になる

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[tier4-autoware-private-bot]

@rej55 🔄 Co-Pilot-Auto Client Triggered... Please wait for the results.

View Client trigger workflow progress: GitHub Actions Run

[tier4-autoware-private-bot]

@rej55 Here's the analysis:

kosuke55様

プルリクエストのレビューをさせていただきます。

全体的な評価

このPRは、回避行動中のプルオーバーパス生成失敗という重要な問題を解決しており、get_reference_lanelets_for_pullover関数の改善を通じて、すべてのプルオーバプランナーがより正確で十分なレーンレット情報を取得できるようになる点で、非常に価値のある変更だと考えます。特に、lane_change_complete_laneからgoal_laneまでのレーンレットを適切に取得し、さらに前後を拡張するロジックは、以前の課題を効果的に解決しているように見えます。

以下に、詳細なレビューコメントと提案を記載します。

1. レーンレット取得ロジックの正確性 (get_reference_lanelets_for_pullover)

1.1. get_reference_lanelets_for_pulloverの変更点の詳細分析

変更されたget_reference_lanelets_for_pullover関数は、find_last_lane_change_completed_laneletが見つからない場合は既存のutils::getExtendedCurrentLanesFromPathを使用し、見つかった場合は新しいロジックでレーンレットを構築しています。

新しいロジックは以下のステップで構成されていると理解しました。

1. lane_change_complete_laneからbackward_length分だけ先行するレーンレットシーケンスを取得。
2. lane_change_complete_laneからgoal_laneまでを接続するレーンレットシーケンスを取得。
3. goal_laneからforward_length分だけ後続するレーンレットシーケンスを取得。
4. これらを結合して最終的なroute_lanesを生成。

このアプローチは、lane_change_complete_laneからgoal_laneまでの連続性を確保しつつ、必要な前後方向の探索範囲をカバーするという点で適切です。

質問:
get_reference_lanelets_for_pulloverの新しい実装において、goal_laneがlane_change_complete_laneから到達不可能、または非常に遠い場合の挙動について、もう少し詳細な説明をいただけますでしょうか？特に、route_handler->get LaneletSequenceやroute_handler->getPrecedingLaneletSequenceがどのように振る舞うか、そしてそれが最終的なroute_lanesにどう影響するかを確認したいです。

1.2. bezier_pull_overへの影響

bezier_pull_overがutils::getExtendedCurrentLanesFromPathの代わりにgoal_planner_utils::get_reference_lanelets_for_pulloverを使用するようになったことで、回避行動中の不適切なレーンレットシーケンスの問題は解決されると期待されます。特に、backward_path_length + backward_search_lengthを渡すことで、パス生成に必要な後方距離とゴール探索に必要な後方距離の両方をカバーしているのは良い変更です。

1.3. エッジケースの考慮

• ゴールレーンが現在のレーンと同じ場合: goal_laneがlane_change_complete_laneと同じ、または非常に近い場合でも、getLaneletSequenceが適切に動作し、必要な最小限のレーンレットが取得されることを確認してください。
• 非常に短い距離: lane_change_complete_laneとgoal_lane間の距離が短い場合でも、backward_lengthとforward_lengthによって十分な探索範囲が確保されることを確認してください。
• 複雑なレーン構造: 合流/分岐レーンを含む複雑なレーン構造の場合、routing_graph->followingやrouting_graph->precedingが期待通りに動作し、ループ検出ロジックが正しく機能することを確認してください。

2. 他のプルオーバプランナーへの影響

ShiftPullOverとGeometricPullOverも同様にget_reference_lanelets_for_pulloverを使用するように変更されています。backward_path_lengthが追加されたことで、これらのプランナーもより堅牢なレーンレット情報を取得できるようになります。

確認事項:
FreeSpacePullOverなど、他のプルオーバプランナーが存在する場合、それらもこの新しいget_reference_lanelets_for_pulloverを使用するように更新されているか、またはその必要がないことを確認してください。PRの説明では「All pull over planners now use this function」とありますが、コード変更はBezierPullOver, GeometricPullOver, ShiftPullOverのみに見えます。

3. 安全性と堅牢性

3.1. boundary_departure_checker_

BezierPullOver::planとGeometricPullOver::planでboundary_departure_checker_が使用されています。lane_departure_check_expansion_marginパラメータが、拡張されたレーンレットに対しても適切に機能し、車両がレーン境界を逸脱しないことを保証できるか確認が必要です。レーンレットが長くなったことで、チェックの計算負荷や精度に影響がないか、特に注意してください。

3.2. 空のレーンレットハンドリング

if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } のチェックは引き続き重要です。新しいレーンレット取得ロジックが、どのような状況でも空のレーンレットを返す可能性があるか、またその場合に適切に処理されるかを確認してください。例えば、goal_laneが見つからない、またはlane_change_complete_laneからgoal_laneへの経路が見つからない場合などです。

4. コードスタイルと可読性

• コメントの更新: get_reference_lanelets_for_pullover関数のDoxygenコメントが非常に詳細に更新されており、関数の意図とパラメータの意味が明確になっています。これは素晴らしい改善です。
• マジックナンバー: 今回の変更で新しいマジックナンバーは導入されていないようです。既存のコードベース全体で、可能な限りマジックナンバーを定数に置き換えることを推奨します。
• 現代的なC++機能: 特に問題は見当たりません。

5. Mermaid Diagram Update

READMEのPlantUML図の更新は、この変更の理解を深める上で非常に役立ちます。get_reference_lanelets_for_pulloverがBezierPullOver、GeometricPullOver、ShiftPullOverにレーンレットを提供する役割を明確に示す図を提案します。

以下にMermaid形式での提案を示します。

classDiagram
    class BehaviorPathPlanner {
        +plan() optional~BehaviorModuleOutput~
    }
    class GoalPlannerModule {
        +plan() optional~BehaviorModuleOutput~
    }
    class PullOverPlannerBase {
        <<abstract>>
        +plan() optional~PullOverPath~
    }
    class BezierPullOver {
        +plan() optional~PullOverPath~
        +plans() vector~PullOverPath~
    }
    class GeometricPullOver {
        +plan() optional~PullOverPath~
    }
    class ShiftPullOver {
        +plan() optional~PullOverPath~
    }
    class GoalPlannerUtils {
        <<static>>
        +get_reference_lanelets_for_pullover(PathWithLaneId, PlannerData, double, double) ConstLanelets
        +getPullOverLanes(RouteHandler, bool, double, double) ConstLanelets
        +find_last_lane_change_completed_lanelet(PathWithLaneId, LaneletMap, RoutingGraph) optional~ConstLanelet~
    }
    class PlannerData {
        +route_handler shared_ptr~RouteHandler~
        +parameters Parameters
    }
    class PathWithLaneId
    class ConstLanelets
    class RouteHandler
    class LaneletMap
    class RoutingGraph

    BehaviorPathPlanner --> GoalPlannerModule
    GoalPlannerModule o-- PullOverPlannerBase
    PullOverPlannerBase <|-- BezierPullOver
    PullOverPlannerBase <|-- GeometricPullOver
    PullOverPlannerBase <|-- ShiftPullOver

    BezierPullOver ..> GoalPlannerUtils : uses
    GeometricPullOver ..> GoalPlannerUtils : uses
    ShiftPullOver ..> GoalPlannerUtils : uses

    GoalPlannerUtils ..> PlannerData : uses
    GoalPlannerUtils ..> PathWithLaneId : uses
    GoalPlannerUtils ..> RouteHandler : uses
    GoalPlannerUtils ..> LaneletMap : uses
    GoalPlannerUtils ..> RoutingGraph : uses

    GoalPlannerUtils --> ConstLanelets : returns
    PlannerData --> RouteHandler

Loading

まとめ

このPRは、回避行動中のプルオーバーパス生成の信頼性を大幅に向上させる重要な修正です。get_reference_lanelets_for_pulloverのロジック改善は適切であり、コメントの更新も非常に良いです。上記の確認事項と提案を考慮していただくことで、さらに堅牢なコードになるかと思います。

ご確認いただけますと幸いです。

[sonarqubecloud]

Quality Gate passed

Issues
2 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud