Skip to content

Commit

Permalink
change to new way of splitting left/right bounds (must rm svg 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 25, 2023
1 parent 0fcf9d0 commit eada8b6
Showing 1 changed file with 191 additions and 72 deletions.
263 changes: 191 additions & 72 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 @@ -21,8 +21,19 @@
#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
#include "interpolation/linear_interpolation.hpp"

#include <Eigen/Geometry>

#include <boost/geometry.hpp>

// for writing the svg file
#include <fstream>
#include <iostream>
// for the geometry types
#include <tier4_autoware_utils/geometry/geometry.hpp>
// for the svg mapper
#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/io/svg/write.hpp>

namespace drivable_area_expansion
{

Expand Down Expand Up @@ -81,59 +92,6 @@ polygon_t createExpandedDrivableAreaPolygon(
return expanded_da_poly;
}

std::array<ring_t::const_iterator, 4> findLeftRightRanges(
const PathWithLaneId & path, const ring_t & expanded_drivable_area)
{
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;
};
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 = [&, size = path.points.size()](const point_t & p) {
return is_left_of_segment(
convert_point(path.points[size - 2].point.pose.position),
convert_point(path.points[size - 1].point.pose.position), p);
};
const auto dist_to_path_start = [start = convert_point(path.points.front().point.pose.position)](
const auto & p) { return boost::geometry::distance(start, p); };
const auto dist_to_path_end = [end = convert_point(path.points.back().point.pose.position)](
const auto & p) { return boost::geometry::distance(end, p); };

double min_start_dist = std::numeric_limits<double>::max();
auto start_transition = expanded_drivable_area.end();
double min_end_dist = std::numeric_limits<double>::max();
auto end_transition = expanded_drivable_area.end();
for (auto it = expanded_drivable_area.begin(); std::next(it) != expanded_drivable_area.end();
++it) {
if (is_left_of_path_start(*it) != is_left_of_path_start(*std::next(it))) {
const auto dist = dist_to_path_start(*it);
if (dist < min_start_dist) {
start_transition = it;
min_start_dist = dist;
}
}
if (is_left_of_path_end(*it) != is_left_of_path_end(*std::next(it))) {
const auto dist = dist_to_path_end(*it);
if (dist < min_end_dist) {
end_transition = it;
min_end_dist = dist;
}
}
}
const auto left_start =
is_left_of_path_start(*start_transition) ? start_transition : std::next(start_transition);
const auto right_start =
is_left_of_path_start(*start_transition) ? std::next(start_transition) : start_transition;
const auto left_end =
is_left_of_path_end(*end_transition) ? end_transition : std::next(end_transition);
const auto right_end =
is_left_of_path_end(*end_transition) ? std::next(end_transition) : end_transition;
return {left_start, left_end, right_start, right_end};
}

void copy_z_over_arc_length(
const std::vector<geometry_msgs::msg::Point> & from, std::vector<geometry_msgs::msg::Point> & to)
{
Expand Down Expand Up @@ -164,32 +122,193 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
{
const auto original_left_bound = path.left_bound;
const auto original_right_bound = path.right_bound;
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;
};

const auto start_segment =
segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())};
const auto end_segment =
segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())};
point_t start_segment_center;
boost::geometry::centroid(start_segment, start_segment_center);
const auto path_start_segment =
segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)};
point_t end_segment_center;
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);
const auto segment = Eigen::Hyperplane<double, 2>::Through(p1, p2);
const auto intersection = line.intersection(segment);
std::optional<point_t> result;
const auto is_on_segment =
(p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x()
: intersection.x() <= p1.x() && intersection.x() >= p2.x()) &&
(p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y()
: intersection.y() <= p1.y() && intersection.y() >= p2.y());
if (is_on_segment) result = point_t{intersection.x(), intersection.y()};
return result;
};
struct Intersection
{
ring_t::const_iterator segment_it;
point_t intersection_point;
double distance = std::numeric_limits<double>::max();
};
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();

// Declare a stream and an SVG mapper
std::ofstream svg("/home/mclement/Pictures/debug.svg");
boost::geometry::svg_mapper<tier4_autoware_utils::Point2d> mapper(svg, 400, 400);
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;
}
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 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;
}
}
}
}
mapper.add(expanded_drivable_area);
mapper.add(path_start_segment);
mapper.add(path_end_segment);
mapper.add(start_segment);
mapper.add(end_segment);
mapper.map(expanded_drivable_area, "fill-opacity:0.0;fill:black;stroke:black;stroke-width:1", 1);
mapper.map(start_segment, "fill-opacity:0.5;fill:blue;stroke:blue;stroke-width:2", 2);
mapper.map(end_segment, "fill-opacity:0.5;fill:blue;stroke:blue;stroke-width:2", 2);
mapper.map(path_start_segment, "fill-opacity:0.5;fill:green;stroke:green;stroke-width:2", 2);
mapper.map(path_end_segment, "fill-opacity:0.5;fill:green;stroke:green;stroke-width:2", 2);
mapper.map(
start_left.intersection_point, "fill-opacity:0.5;fill:red;stroke:red;stroke-width:2", 2);
mapper.map(
start_right.intersection_point, "fill-opacity:0.5;fill:red;stroke:red;stroke-width:2", 2);
mapper.map(end_left.intersection_point, "fill-opacity:0.5;fill:red;stroke:red;stroke-width:2", 2);
mapper.map(
end_right.intersection_point, "fill-opacity:0.5;fill:red;stroke:red;stroke-width:2", 2);

if (
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;
}

path.left_bound.clear();
path.right_bound.clear();
const auto begin = expanded_drivable_area.outer().begin();
const auto end = std::prev(expanded_drivable_area.outer().end());
const auto & [left_start, left_end, right_start, right_end] =
findLeftRightRanges(path, expanded_drivable_area.outer());
// NOTE: clockwise ordering -> positive increment for left bound, negative for right bound
if (left_start < left_end) {
path.left_bound.reserve(std::distance(left_start, left_end));
for (auto it = left_start; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
} else { // loop back
path.left_bound.reserve(std::distance(left_start, end) + std::distance(begin, left_end));
for (auto it = left_start; it != end; ++it) path.left_bound.push_back(convert_point(*it));
for (auto it = begin; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
path.left_bound.push_back(convert_point(start_left.intersection_point));
path.right_bound.push_back(convert_point(start_right.intersection_point));
if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it))
path.right_bound.push_back(convert_point(*start_right.segment_it));
if (start_left.segment_it < end_left.segment_it) {
for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it)
path.left_bound.push_back(convert_point(*it));
} else {
for (auto it = std::next(start_left.segment_it); it < da.end(); ++it)
path.left_bound.push_back(convert_point(*it));
for (auto it = da.begin(); it <= end_left.segment_it; ++it)
path.left_bound.push_back(convert_point(*it));
}
if (right_start > right_end) {
path.right_bound.reserve(std::distance(right_end, right_start));
for (auto it = right_start; it >= right_end; --it)
if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it))
path.left_bound.push_back(convert_point(end_left.intersection_point));
if (start_right.segment_it < end_right.segment_it) {
for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it)
path.right_bound.push_back(convert_point(*it));
for (auto it = std::prev(da.end()); it > end_right.segment_it; --it)
path.right_bound.push_back(convert_point(*it));
} else {
for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it)
path.right_bound.push_back(convert_point(*it));
} else { // loop back
path.right_bound.reserve(std::distance(begin, right_start) + std::distance(right_end, end));
for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it));
for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it));
}
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;
};
std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp);
std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp);
copy_z_over_arc_length(original_left_bound, path.left_bound);
copy_z_over_arc_length(original_right_bound, path.right_bound);

for (const auto & p : path.right_bound) {
mapper.add(convert_point(p));
mapper.map(convert_point(p), "fill-opacity:0.5;fill:grey;stroke:grey;stroke-width:2", 1);
}
}

} // namespace drivable_area_expansion

0 comments on commit eada8b6

Please sign in to comment.