Skip to content

Commit

Permalink
Cleanup the new code
Browse files Browse the repository at this point in the history 
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
  • Loading branch information
@maxime-clem
maxime-clem committed Jul 26, 2023
1 parent b0afaaf commit 928522a
Showing 1 changed file with 38 additions and 71 deletions.
109 changes: 38 additions & 71 deletions planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) {
return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0;
};

// prepare delimiting lines: start and end of the original expanded drivable area
const auto start_segment =
segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())};
const auto end_segment =
Expand All @@ -129,15 +129,6 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
boost::geometry::centroid(end_segment, end_segment_center);
const auto path_end_segment =
segment_t{convert_point(path.points.back().point.pose.position), end_segment_center};
const auto is_left_of_path_start = [&](const point_t & p) {
return is_left_of_segment(
convert_point(path.points[0].point.pose.position),
convert_point(path.points[1].point.pose.position), p);
};
const auto is_left_of_path_end = [&](const point_t & p) {
return is_left_of_segment(
convert_point(path.points.back().point.pose.position), end_segment_center, p);
};
const auto segment_to_line_intersection =
[](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional<point_t> {
const auto line = Eigen::Hyperplane<double, 2>::Through(q1, q2);
Expand All @@ -152,91 +143,69 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
if (is_on_segment) result = point_t{intersection.x(), intersection.y()};
return result;
};
// find intersection between the expanded drivable area and the delimiting lines
const auto & da = expanded_drivable_area.outer();
struct Intersection
{
ring_t::const_iterator segment_it;
point_t intersection_point;
ring_t::const_iterator segment_it;
double distance = std::numeric_limits<double>::max();
explicit Intersection(ring_t::const_iterator it) : segment_it(it) {}
void update(const point_t & p, const ring_t::const_iterator & it, const double dist)
{
intersection_point = p;
segment_it = it;
distance = dist;
}
};
const auto & da = expanded_drivable_area.outer();
Intersection start_left;
start_left.segment_it = da.end();
Intersection end_left;
end_left.segment_it = da.end();
Intersection start_right;
start_right.segment_it = da.end();
Intersection end_right;
end_right.segment_it = da.end();

Intersection start_left(da.end());
Intersection end_left(da.end());
Intersection start_right(da.end());
Intersection end_right(da.end());
for (auto it = da.begin(); it != da.end(); ++it) {
if (boost::geometry::distance(*it, start_segment.first) < 1e-3) {
start_left.intersection_point = *it;
start_left.segment_it = it;
start_left.distance = 0.0;
} else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) {
start_right.intersection_point = *it;
start_right.segment_it = it;
start_right.distance = 0.0;
} else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) {
end_left.intersection_point = *it;
end_left.segment_it = it;
end_left.distance = 0.0;
} else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) {
end_right.intersection_point = *it;
end_right.segment_it = it;
end_right.distance = 0.0;
}
if (boost::geometry::distance(*it, start_segment.first) < 1e-3)
start_left.update(*it, it, 0.0);
else if (boost::geometry::distance(*it, start_segment.second) < 1e-3)
start_right.update(*it, it, 0.0);
else if (boost::geometry::distance(*it, end_segment.first) < 1e-3)
end_left.update(*it, it, 0.0);
else if (boost::geometry::distance(*it, end_segment.second) < 1e-3)
end_right.update(*it, it, 0.0);
const auto inter_start =
std::next(it) == da.end()
? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second)
: segment_to_line_intersection(
*it, *std::next(it), start_segment.first, start_segment.second);
if (inter_start) {
const auto dist = boost::geometry::distance(*inter_start, path_start_segment);
if (is_left_of_path_start(*inter_start)) {
if (dist < start_left.distance) {
start_left.intersection_point = *inter_start;
start_left.segment_it = it;
start_left.distance = dist;
}
} else {
if (dist < start_right.distance) {
start_right.intersection_point = *inter_start;
start_right.segment_it = it;
start_right.distance = dist;
}
}
const auto is_left_of_path_start = is_left_of_segment(
convert_point(path.points[0].point.pose.position),
convert_point(path.points[1].point.pose.position), *inter_start);
if (is_left_of_path_start && dist < start_left.distance)
start_left.update(*inter_start, it, dist);
else if (!is_left_of_path_start && dist < start_right.distance)
start_right.update(*inter_start, it, dist);
}
const auto inter_end =
std::next(it) == da.end()
? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second)
: segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second);
if (inter_end) {
const auto dist = boost::geometry::distance(*inter_end, path_end_segment);
if (is_left_of_path_end(*inter_end)) {
if (dist < end_left.distance) {
end_left.intersection_point = *inter_end;
end_left.segment_it = it;
end_left.distance = dist;
}
} else {
if (dist < end_right.distance) {
end_right.intersection_point = *inter_end;
end_right.segment_it = it;
end_right.distance = dist;
}
}
const auto is_left_of_path_end = is_left_of_segment(
convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end);
if (is_left_of_path_end && dist < end_left.distance)
end_left.update(*inter_end, it, dist);
else if (!is_left_of_path_end && dist < end_right.distance)
end_right.update(*inter_end, it, dist);
}
}
if (
if ( // illformed expanded drivable area -> keep the original bounds
start_left.segment_it == da.end() || start_right.segment_it == da.end() ||
end_left.segment_it == da.end() || end_right.segment_it == da.end()) {
std::cerr << (start_left.segment_it == da.end()) << " " << (start_right.segment_it == da.end())
<< " " << (end_left.segment_it == da.end()) << " "
<< (end_right.segment_it == da.end()) << std::endl;
return;
}

// extract the expanded left and right bound from the expanded drivable area
path.left_bound.clear();
path.right_bound.clear();
path.left_bound.push_back(convert_point(start_left.intersection_point));
Expand Down Expand Up @@ -265,7 +234,6 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
}
if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it)))
path.right_bound.push_back(convert_point(end_right.intersection_point));

// remove possible duplicated points
const auto point_cmp = [](const auto & p1, const auto & p2) {
return p1.x == p2.x && p1.y == p2.y;
Expand All @@ -275,5 +243,4 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
copy_z_over_arc_length(original_left_bound, path.left_bound);
copy_z_over_arc_length(original_right_bound, path.right_bound);
}

} // namespace drivable_area_expansion

0 comments on commit 928522a

Please sign in to comment.