Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(avoidance): check if the avoidance path is in drivable area #5032

Merged

Conversation

satoshi-ota
Copy link
Contributor

@satoshi-ota satoshi-ota commented Sep 19, 2023

Description

🤖 Generated by Copilot at 3d133af

This pull request refactors and improves the avoidance module of the behavior path planner. It removes unused or redundant code, adds new variables and parameters to store and control the drivable area and the lane departure, and enhances the generation and validation of shift lines for obstacle avoidance. These changes aim to increase the safety and efficiency of the avoidance behavior.

This PR related to autowarefoundation/autoware_launch#584.


  // check if the vehicle is in road.
  {
    const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width +
                                  parameters_->hard_road_shoulder_margin -
                                  parameters_->max_departure_from_lane;

    const size_t start_idx = shift_lines.front().start_idx;
    const size_t end_idx = shift_lines.back().end_idx;

    for (size_t i = start_idx; i <= end_idx; ++i) {
      const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i));
      lanelet::BasicPoint2d basic_point{p.x, p.y};

      const auto shift_length = proposed_shift_path.shift_length.at(i);
      const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound;
      const auto THRESHOLD = minimum_distance + std::abs(shift_length);

      if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) {
        RCLCPP_DEBUG_THROTTLE(getLogger(), *clock_, 1000, "road shoulder distance is not enough.");
        return false;
      }
    }
  }

Tests performed

Effects on system behavior

Nothing.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

@github-actions github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label Sep 19, 2023
@satoshi-ota satoshi-ota force-pushed the feat/validate-avoidance-path branch 6 times, most recently from 97343ae to b60fe82 Compare September 22, 2023 06:21
@satoshi-ota satoshi-ota marked this pull request as ready for review September 22, 2023 09:16
@kyoichi-sugahara kyoichi-sugahara self-assigned this Oct 2, 2023
@@ -235,6 +235,9 @@ struct AvoidanceParameters
// Even if the obstacle is very large, it will not avoid more than this length for left direction
double max_left_shift_length{0.0};

// Validate vehicle departure from driving lane.
double max_departure_from_lane{0.0};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[NITS] by sugaharaGPT
max_deviation_from_lane?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks for your idea. Fixed.

@@ -2350,6 +2376,30 @@ bool AvoidanceModule::isValidShiftLine(
}
}

// check if the vehicle is in road.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// check if the vehicle is in road.
// check if the vehicle is in road. (yaw angle is not considered)

const auto THRESHOLD = minimum_distance + std::abs(shift_length);

if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) {
RCLCPP_DEBUG_THROTTLE(getLogger(), *clock_, 1000, "road shoulder distance is not enough.");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It seems this debug info is outputted even if road shoulder distance is big enough?
In my understanding, this condition is met when road shoulder distance is not enough or max_departure_from_lane is big?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

update comment!

following avoidance path that is generated from latest new shift line may cause deviation from drivable area.

bool safe{false};

bool valid{false};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what does valid means? or invalid means?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

valid includes some criterias:

  • deviation from current ego position
  • deviation from current lane
  • path curverture (doesn't exist for now)
  • ...

Since I will add some validation functions, I used general name.

Copy link
Contributor

@kyoichi-sugahara kyoichi-sugahara left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM!

@satoshi-ota satoshi-ota force-pushed the feat/validate-avoidance-path branch from b60fe82 to 37573a1 Compare October 11, 2023 07:23
@satoshi-ota satoshi-ota added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Oct 11, 2023
@codecov
Copy link

codecov bot commented Oct 11, 2023

Codecov Report

Attention: 43 lines in your changes are missing coverage. Please review.

Comparison is base (bc1d6c7) 14.83% compared to head (1850a4e) 14.87%.
Report is 4 commits behind head on main.

Additional details and impacted files
@@            Coverage Diff             @@
##             main    #5032      +/-   ##
==========================================
+ Coverage   14.83%   14.87%   +0.03%     
==========================================
  Files        1649     1649              
  Lines      114261   114325      +64     
  Branches    35368    35405      +37     
==========================================
+ Hits        16956    17001      +45     
+ Misses      78167    78157      -10     
- Partials    19138    19167      +29     
Flag Coverage Δ *Carryforward flag
differential 12.56% <21.81%> (?)
total 14.84% <ø> (+<0.01%) ⬆️ Carriedforward from bc1d6c7

*This pull request uses carry forward flags. Click here to find out more.

Files Coverage Δ
...lanner/scene_module/avoidance/avoidance_module.hpp 7.81% <ø> (ø)
..._planner/utils/avoidance/avoidance_module_data.hpp 0.00% <ø> (ø)
planning/obstacle_cruise_planner/src/node.cpp 12.42% <ø> (+0.01%) ⬆️
...ath_planner/src/scene_module/avoidance/manager.cpp 5.94% <0.00%> (-0.05%) ⬇️
...ehavior_path_planner/src/utils/avoidance/utils.cpp 11.55% <0.00%> (+3.46%) ⬆️
planning/behavior_path_planner/src/utils/utils.cpp 13.56% <0.00%> (-0.03%) ⬇️
...er/src/scene_module/avoidance/avoidance_module.cpp 12.85% <25.53%> (+0.62%) ⬆️

... and 3 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@satoshi-ota satoshi-ota force-pushed the feat/validate-avoidance-path branch 2 times, most recently from add619c to 6e0a072 Compare October 12, 2023 08:07
@satoshi-ota satoshi-ota force-pushed the feat/validate-avoidance-path branch from 28269c4 to 1850a4e Compare October 12, 2023 08:27
@satoshi-ota satoshi-ota merged commit 75debf0 into autowarefoundation:main Oct 12, 2023
22 of 23 checks passed
@satoshi-ota satoshi-ota deleted the feat/validate-avoidance-path branch October 12, 2023 09:02
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci)
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants