Skip to content

Commit

Permalink
fix(behavior velocity planner): occlusion spot slice (#237)
Browse files Browse the repository at this point in the history 
* fix: occlusion spot slice

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore: use to path lanelet

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix: pre-commit

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore: minor change for debug and test

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(gtest): update to latest and minor fix of edge case

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(typo): fix typo

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
  • Loading branch information
@taikitanaka3 @TomohitoAndo
taikitanaka3 authored and TomohitoAndo committed Jan 29, 2022
1 parent 1614c3c commit c8a4e5c
Show file tree
Hide file tree
Showing 8 changed files with 159 additions and 320 deletions.
14 changes: 5 additions & 9 deletions planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,16 +49,12 @@ struct Slice
// using the given range and desired slice length and width
void buildSlices(
std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
const double slice_length, const double slice_width);
//!< @brief build an interpolated polygon between the given bounds
void buildInterpolatedPolygon(
lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & from_bound,
const lanelet::BasicLineString2d & to_bound, const double from_length, const double to_length,
const double from_ratio_dist, const double to_ratio_dist);
const double slice_length, const double slice_width, const double resolution);
//!< @brief build sidewalk slice from path
std::vector<geometry::Slice> buildSidewalkSlices(
const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset,
const double lateral_offset, const double min_size, const double lateral_max_dist);
void buildSidewalkSlices(
std::vector<geometry::Slice> & slice, const lanelet::ConstLanelet & path_lanelet,
const double longitudinal_offset, const double lateral_offset, const double min_size,
const double lateral_max_dist);
//!< @brief calculate interpolation between a and b at distance ratio t
template <typename T>
T lerp(T a, T b, double t)
Expand Down
1 change: 1 addition & 0 deletions ...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

namespace behavior_velocity_planner
{
using autoware_auto_planning_msgs::msg::PathWithLaneId;
namespace occlusion_spot_utils
{
enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,6 +194,7 @@ visualization_msgs::msg::MarkerArray createMarkers(
occlusion_spot_slowdown_markers.markers.insert(
occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(),
collision_markers.end());
break;
}

// draw slice if having sidewalk polygon
Expand Down
231 changes: 79 additions & 152 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,76 +24,80 @@ namespace behavior_velocity_planner
{
namespace geometry
{
using lanelet::BasicLineString2d;
using lanelet::BasicPoint2d;
using lanelet::BasicPolygon2d;
namespace bg = boost::geometry;
namespace lg = lanelet::geometry;

void createOffsetLineString(
const BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string)
{
for (size_t i = 0; i < in.size() - 1; i++) {
const auto & p0 = in.at(i);
const auto & p1 = in.at(i + 1);
// translation
const double dy = p1[1] - p0[1];
const double dx = p1[0] - p0[0];
// rotation (use inverse matrix of rotation)
const double yaw = std::atan2(dy, dx);
// translation
const double offset_x = p0[0] - std::sin(yaw) * offset;
const double offset_y = p0[1] + std::cos(yaw) * offset;
offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
//! insert final offset linestring using prev vertical direction
if (i == in.size() - 2) {
const double offset_x = p1[0] - std::sin(yaw) * offset;
const double offset_y = p1[1] + std::cos(yaw) * offset;
offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
}
}
return;
}

void buildSlices(
std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
const double slice_length, const double slice_width)
const double slice_length, const double slice_width, const double resolution)
{
lanelet::BasicLineString2d path_boundary = path_lanelet.centerline2d().basicLineString();
if (path_boundary.size() < 2) {
return;
}
const int num_lateral_slice =
static_cast<int>(std::abs(range.max_distance - range.min_distance) / slice_width);
// ignore the last point
const int num_longitudinal_slice =
static_cast<int>(std::abs(range.max_length - range.min_length) / slice_length);
slices.reserve(num_lateral_slice * num_longitudinal_slice);
// Note: offsetNoThrow is necessary
// as the sharp turn at the end of the trajectory can "reverse" the linestring
/**
* @brief
* build slice from occupancy grid : the first slice to create is ssss
*
* | 0 | 1 | 2 | 3 | 4 |
* 0 | --|---|---|---|---|-- outer bound
* 1 | | ? | ? | | |
* 2 | | ? | ? | | |
* 3 | | ? | ? | | | ^
* 4 | s | s | ? | | | | dist_ratio
* 5 |-s-|-s-|---|---|---|-- inner bound --> length ratio
* Ego---------------------> total_slice_length
* @brief bounds
* +---------- outer bounds
* | +------ inner bounds(original path)
* | |
*/
lanelet::BasicLineString2d inner_bounds = path_boundary;
lanelet::BasicLineString2d outer_bounds;
outer_bounds = lanelet::geometry::offsetNoThrow(path_boundary, range.max_distance);
// correct self crossing with best effort
boost::geometry::correct(outer_bounds);
rclcpp::Clock clock{RCL_ROS_TIME};
try {
// if correct couldn't solve self crossing skip this area
lanelet::geometry::internal::checkForInversion(
path_boundary, outer_bounds, std::abs(range.max_distance));
} catch (...) {
RCLCPP_DEBUG_STREAM_THROTTLE(
rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), clock, 5000,
"self crossing with offset " << range.max_distance);
}
// offset last point is especially messy
inner_bounds.pop_back();
outer_bounds.pop_back();
// resize to the same size as slice
inner_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size()));
outer_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size()));
if (inner_bounds.size() < 2 || outer_bounds.size() < 2) {
return;
}
BasicLineString2d inner_bounds = path_lanelet.centerline2d().basicLineString();
BasicLineString2d outer_bounds;
if (inner_bounds.size() < 2) return;
createOffsetLineString(inner_bounds, range.max_distance, outer_bounds);
const double ratio_dist_start = std::abs(range.min_distance / range.max_distance);
const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance));
for (int s = 0; s < num_longitudinal_slice; s++) {
const double length = range.min_length + s * slice_length;
const double next_length = range.min_length + (s + 1.0) * slice_length;
const double min_length =
std::min(lanelet::geometry::length(outer_bounds), lanelet::geometry::length(inner_bounds));
if (next_length > min_length) {
break;
}
lanelet::BasicPolygon2d poly;
const int num_step = static_cast<int>(slice_length / resolution);
//! max index is the last index of path point
const int max_index = static_cast<int>(inner_bounds.size() - 1);
for (int s = 0; s < max_index; s += num_step) {
const double length = s * slice_length;
const double next_length = (s + num_step) * resolution;
for (int d = 0; d < num_lateral_slice; d++) {
const double ratio_dist = ratio_dist_start + d * ratio_dist_increment;
const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment;
Slice slice;
buildInterpolatedPolygon(
slice.polygon, inner_bounds, outer_bounds, length, next_length, ratio_dist,
next_ratio_dist);
BasicLineString2d inner_polygons;
BasicLineString2d outer_polygons;
// build interpolated polygon for lateral
for (int i = 0; i <= num_step; i++) {
if (s + i >= max_index) continue;
inner_polygons.emplace_back(
lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist));
outer_polygons.emplace_back(
lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist));
}
// Build polygon
inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend());
slice.polygon = lanelet::BasicPolygon2d(inner_polygons);
// add range info
slice.range.min_length = length;
slice.range.max_length = next_length;
slice.range.min_distance = ratio_dist * range.max_distance;
Expand All @@ -103,106 +107,29 @@ void buildSlices(
}
}

void buildInterpolatedPolygon(
lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & inner_bounds,
const lanelet::BasicLineString2d & outer_bounds, const double current_length,
const double next_length, const double from_ratio_dist, const double to_ratio_dist)
void buildSidewalkSlices(
std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet,
const double longitudinal_offset, const double lateral_offset, const double slice_size,
const double lateral_max_dist)
{
if (current_length >= next_length) {
RCLCPP_WARN(
rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"),
"buildInterpolatedPolygon: starting length must be lower than target length");
}
lanelet::BasicLineString2d inner_polygons;
lanelet::BasicLineString2d outer_polygons;
inner_polygons.reserve(inner_bounds.size());
outer_polygons.reserve(outer_bounds.size());
// Find starting point. Interpolate it if necessary
double length = 0.0;
size_t point_index = 0;
lanelet::BasicPoint2d inner_polygon_from;
lanelet::BasicPoint2d inner_polygon_to;
lanelet::BasicPoint2d outer_polygon_from;
lanelet::BasicPoint2d outer_polygon_to;
// Search first points of polygon
for (; length < current_length && point_index < inner_bounds.size() - 1; ++point_index) {
length +=
lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
}
// if find current bound point index
if (length > current_length) {
const double length_between_points =
lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]);
const double length_ratio =
(length_between_points - (length - current_length)) / length_between_points;
inner_polygon_from =
lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio);
outer_polygon_from =
lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio);
} else {
inner_polygon_from = inner_bounds[point_index];
outer_polygon_from = outer_bounds[point_index];
if (length < next_length && point_index < inner_bounds.size() - 1) {
length +=
lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
++point_index;
}
}
inner_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, from_ratio_dist));
outer_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, to_ratio_dist));

// Intermediate points
for (; length < next_length && point_index < inner_bounds.size() - 1; ++point_index) {
inner_polygons.emplace_back(
lerp(inner_bounds[point_index], outer_bounds[point_index], from_ratio_dist));
outer_polygons.emplace_back(
lerp(inner_bounds[point_index], outer_bounds[point_index], to_ratio_dist));
length +=
lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
}
// Last points
if (length > next_length) {
const double length_between_points =
lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]);
const double length_ratio =
(length_between_points - (length - next_length)) / length_between_points;
inner_polygon_to = lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio);
outer_polygon_to = lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio);
} else {
inner_polygon_to = inner_bounds[point_index];
outer_polygon_to = outer_bounds[point_index];
}
inner_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, from_ratio_dist));
outer_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, to_ratio_dist));
// Build polygon
inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend());
polygons = lanelet::BasicPolygon2d(inner_polygons);
}

std::vector<geometry::Slice> buildSidewalkSlices(
const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset,
const double lateral_offset, const double slice_size, const double lateral_max_dist)
{
std::vector<geometry::Slice> slices;
std::vector<geometry::Slice> left_slices;
std::vector<geometry::Slice> right_slices;
const double longitudinal_max_dist = lanelet::geometry::length2d(path_lanelet);
geometry::SliceRange left_slice_range = {
std::vector<Slice> left_slices;
std::vector<Slice> right_slices;
const double longitudinal_max_dist = lg::length2d(path_lanelet);
SliceRange left_slice_range = {
longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist};
geometry::buildSlices(left_slices, path_lanelet, left_slice_range, slice_size, slice_size);
geometry::SliceRange right_slice_range = {
// in most case lateral distance is much more effective for velocity planning
const double slice_length = 4.0 * slice_size;
const double slice_width = slice_size;
const double resolution = 1.0;
buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution);
SliceRange right_slice_range = {
longitudinal_offset, longitudinal_max_dist, -lateral_offset,
-lateral_offset - lateral_max_dist};
geometry::buildSlices(right_slices, path_lanelet, right_slice_range, slice_size, slice_size);
buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution);
// Properly order lanelets from closest to furthest
for (size_t i = 0; i < right_slices.size(); ++i) {
slices.emplace_back(right_slices[i]);
}
for (size_t i = 0; i < left_slices.size(); ++i) {
slices.emplace_back(left_slices[i]);
}

return slices;
slices = left_slices;
slices.insert(slices.end(), right_slices.begin(), right_slices.end());
return;
}
} // namespace geometry
} // namespace behavior_velocity_planner
23 changes: 19 additions & 4 deletions planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,20 @@ namespace behavior_velocity_planner
{
namespace occlusion_spot_utils
{
lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path)
{
lanelet::Points3d path_points;
path_points.reserve(path.points.size());
for (const auto & path_point : path.points) {
const auto & p = path_point.point.pose.position;
path_points.emplace_back(lanelet::InvalId, p.x, p.y, p.z);
}
lanelet::LineString3d centerline(lanelet::InvalId, path_points);
lanelet::Lanelet path_lanelet(lanelet::InvalId);
path_lanelet.setCenterline(centerline);
return lanelet::ConstLanelet(path_lanelet);
}

bool splineInterpolate(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
Expand Down Expand Up @@ -419,7 +433,7 @@ void generatePossibleCollisions(
const PlannerParam & param, std::vector<lanelet::BasicPolygon2d> & debug)
{
// NOTE : buildPathLanelet first index should always be zero because path is already limited
lanelet::ConstLanelet path_lanelet = buildPathLanelet(path);
lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
if (path_lanelet.centerline2d().empty()) {
return;
}
Expand All @@ -446,9 +460,10 @@ void generateSidewalkPossibleCollisions(
} else {
min_range = param.vehicle_info.baselink_to_front - offset_form_ego_to_target;
}
std::vector<geometry::Slice> sidewalk_slices = geometry::buildSidewalkSlices(
path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, param.sidewalk.slice_size,
param.sidewalk.focus_range);
std::vector<geometry::Slice> sidewalk_slices;
geometry::buildSidewalkSlices(
sidewalk_slices, path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5,
param.sidewalk.slice_size, param.sidewalk.focus_range);
double length_lower_bound = std::numeric_limits<double>::max();
double distance_lower_bound = std::numeric_limits<double>::max();
std::sort(
Expand Down
Loading

0 comments on commit c8a4e5c

Please sign in to comment.