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.

Sign up for GitHub

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

fix(avoidance): discard envelope polygon if the objects move long distance #5388

Merged
merged 3 commits into from
Oct 31, 2023
Merged

fix(avoidance): discard envelope polygon if the objects move long distance #5388

Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
bde24c1
fix(avoidance): discard envelope polygon if the objects move long dis…
satoshi-ota Oct 20, 2023
73eb109
Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp
satoshi-ota Oct 31, 2023
cdd282a
Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp
satoshi-ota Oct 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 19 additions & 6 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -679,26 +679,39 @@ void fillObjectEnvelopePolygon(
return;
}

const auto envelope_poly =
const auto one_shot_envelope_poly =
createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);

if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) {
if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) {
satoshi-ota marked this conversation as resolved.
Show resolved Hide resolved
object_data.envelope_poly = same_id_obj->envelope_poly;
return;
}

std::vector<Polygon2d> unions;
boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions);
boost::geometry::union_(one_shot_envelope_poly, same_id_obj->envelope_poly, unions);

if (unions.empty()) {
satoshi-ota marked this conversation as resolved.
Show resolved Hide resolved
object_data.envelope_poly =
createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin);
object_data.envelope_poly = one_shot_envelope_poly;
return;
}

boost::geometry::correct(unions.front());

object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0);
const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0);

const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object);
const auto object_polygon_area = boost::geometry::area(object_polygon);
const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly);

// keep multi-step envelope polygon.
constexpr double THRESHOLD = 5.0;
if (envelope_polygon_area < object_polygon_area * THRESHOLD) {
object_data.envelope_poly = multi_step_envelope_poly;
return;
}

// use latest one-shot envelope polygon.
object_data.envelope_poly = one_shot_envelope_poly;
}

void fillObjectMovingTime(
Expand Down