Skip to content

Commit

Permalink
[TMP] trying new way to split left/right bounds (WIP)
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 24, 2023
1 parent 10f9d6b commit 76e475c
Showing 1 changed file with 147 additions and 73 deletions.
220 changes: 147 additions & 73 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 @@ -23,6 +23,15 @@

#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 +90,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 @@ -166,27 +122,145 @@ void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_
const auto original_right_bound = path.right_bound;
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));
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);
};
// trim points beyond the original drivable area
const auto segment_to_line_intersection =
[](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional<point_t> {
const auto d = q2 - q1;
const auto t = (q1 - p1).dot(d) / d.dot(d);

std::optional<point_t> result;
if (t >= 0 && t <= 1.0) {
const auto p = p1 + t * (p2 - p1);
result = point_t{p.x(), p.y()};
}
return result;
};
struct Intersection
{
ring_t::const_iterator segment_it;
point_t intersection_point;
double distance;
};
std::vector<Intersection> start_intersections;
std::vector<Intersection> end_intersections;

// 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);

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())};
mapper.add(expanded_drivable_area);
// mapper.add(start_segment);
// mapper.add(end_segment);
mapper.map(expanded_drivable_area, "fill-opacity:0.5;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);
const auto & da = expanded_drivable_area.outer();
for (auto it = da.begin(); it != da.end(); ++it) {
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) {
Intersection inter;
inter.intersection_point = *inter_start;
inter.segment_it = it;
inter.distance = boost::geometry::distance(
*inter_start, convert_point(path.points.front().point.pose.position));
start_intersections.push_back(inter);
// mapper.add(*inter_start);
// mapper.map(*inter_start, "fill-opacity:0.5;fill:red;stroke:red;stroke-width:2", 2);
}
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) {
Intersection inter;
inter.intersection_point = *inter_end;
inter.segment_it = it;
inter.distance = boost::geometry::distance(
*inter_start, convert_point(path.points.back().point.pose.position));
end_intersections.push_back(inter);
}
}
if (right_start > right_end) {
path.right_bound.reserve(std::distance(right_end, right_start));
for (auto it = right_start; it >= right_end; --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));
std::cerr << "intersections : " << start_intersections.size() << " " << end_intersections.size()
<< std::endl;
std::sort(
start_intersections.begin(), start_intersections.end(),
[](const auto & i1, const auto & i2) { return i1.distance < i2.distance; });
std::sort(
end_intersections.begin(), end_intersections.end(),
[](const auto & i1, const auto & i2) { return i1.distance < i2.distance; });
std::unique(
start_intersections.begin(), start_intersections.end(), [](const auto & i1, const auto & i2) {
return boost::geometry::equals(i1.intersection_point, i2.intersection_point);
});
std::unique(
end_intersections.begin(), end_intersections.end(), [](const auto & i1, const auto & i2) {
return boost::geometry::equals(i1.intersection_point, i2.intersection_point);
});
if (start_intersections.size() > 1) {
const auto left_start = is_left_of_path_start(start_intersections[0].intersection_point)
? start_intersections[0]
: start_intersections[1];
const auto right_start = is_left_of_path_start(start_intersections[0].intersection_point)
? start_intersections[1]
: start_intersections[0];
if (end_intersections.size() > 1) {
const auto left_end = is_left_of_path_end(end_intersections[0].intersection_point)
? end_intersections[0]
: end_intersections[1];
const auto right_end = is_left_of_path_end(end_intersections[0].intersection_point)
? end_intersections[1]
: end_intersections[0];
path.left_bound.push_back(convert_point(left_start.intersection_point));
path.right_bound.push_back(convert_point(right_start.intersection_point));
if (!boost::geometry::equals(
left_start.intersection_point, *std::next(left_start.segment_it)))
path.left_bound.push_back(convert_point(*std::next(left_start.segment_it)));
if (!boost::geometry::equals(right_start.intersection_point, *right_start.segment_it))
path.right_bound.push_back(convert_point(*right_start.segment_it));
if (left_start.segment_it < left_end.segment_it) {
for (auto it = std::next(left_start.segment_it); it <= left_end.segment_it; ++it)
path.left_bound.push_back(convert_point(*it));
} else {
for (auto it = std::next(left_start.segment_it); it <= da.end(); ++it)
path.left_bound.push_back(convert_point(*it));
for (auto it = da.begin(); it <= left_end.segment_it; ++it)
path.left_bound.push_back(convert_point(*it));
}
if (!boost::geometry::equals(left_end.intersection_point, *left_end.segment_it))
path.left_bound.push_back(convert_point(left_end.intersection_point));
if (right_start.segment_it < right_end.segment_it) {
for (auto it = right_start.segment_it; it >= da.begin(); --it)
path.right_bound.push_back(convert_point(*it));
for (auto it = std::prev(da.end()); it > right_end.segment_it; --it)
path.right_bound.push_back(convert_point(*it));
} else {
for (auto it = right_start.segment_it; it > right_end.segment_it; --it)
path.right_bound.push_back(convert_point(*it));
}
if (!boost::geometry::equals(right_end.intersection_point, *std::next(right_end.segment_it)))
path.right_bound.push_back(convert_point(right_end.intersection_point));
}
}
copy_z_over_arc_length(original_left_bound, path.left_bound);
copy_z_over_arc_length(original_right_bound, path.right_bound);
Expand Down

0 comments on commit 76e475c

Please sign in to comment.