From dd5275c2cc41cdd6fa392e1c7125b587e1acdd68 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Fri, 13 Jan 2023 11:35:09 +0900 Subject: [PATCH] fix(obstacle_avoidance_planner): fix drivable area checker (#2639) (#243) * fix(obstacle_avoidance_planner): fix drivable area checker Signed-off-by: yutaka * fix format Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: yutaka Co-authored-by: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> --- .../utils/utils.hpp | 4 +- .../src/utils/utils.cpp | 74 ++++++++++++++----- 2 files changed, 57 insertions(+), 21 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp index 1f1b3dbdf9249..fd8a8f88e054d 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/utils.hpp @@ -358,8 +358,8 @@ namespace drivable_area_utils { bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector left_bound, - const std::vector right_bound, const VehicleParam & vehicle_param); + const std::vector & left_bound, + const std::vector & right_bound, const VehicleParam & vehicle_param); } #endif // OBSTACLE_AVOIDANCE_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp index ad7257e907047..bb74a3e57677c 100644 --- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp @@ -32,6 +32,10 @@ #include #include +namespace bg = boost::geometry; +typedef bg::model::d2::point_xy bg_point; +typedef bg::model::polygon bg_polygon; + namespace { std::vector convertEulerAngleToMonotonic(const std::vector & angle) @@ -628,7 +632,7 @@ geometry_msgs::msg::Point getStartPoint( return bound.front(); } -bool isOutsideDrivableArea( +bool isFrontDrivableArea( const geometry_msgs::msg::Point & point, const std::vector & left_bound, const std::vector & right_bound) @@ -644,23 +648,33 @@ bool isOutsideDrivableArea( // ignore point behind of the front line const std::vector front_bound = {left_start_point, right_start_point}; const double lat_dist_to_front_bound = motion_utils::calcLateralOffset(front_bound, point); - if (lat_dist_to_front_bound < -min_dist) { - return false; + if (lat_dist_to_front_bound < min_dist) { + return true; } - // left bound check - const double lat_dist_to_left_bound = motion_utils::calcLateralOffset(left_bound, point); - if (lat_dist_to_left_bound > min_dist) { - return true; + return false; +} + +bg_polygon createDrivablePolygon( + const std::vector & left_bound, + const std::vector & right_bound) +{ + bg_polygon drivable_area_poly; + + // right bound + for (const auto rp : right_bound) { + drivable_area_poly.outer().push_back(bg_point(rp.x, rp.y)); } - // right bound check - const double lat_dist_to_right_bound = motion_utils::calcLateralOffset(right_bound, point); - if (lat_dist_to_right_bound < -min_dist) { - return true; + // left bound + auto reversed_left_bound = left_bound; + std::reverse(reversed_left_bound.begin(), reversed_left_bound.end()); + for (const auto lp : reversed_left_bound) { + drivable_area_poly.outer().push_back(bg_point(lp.x, lp.y)); } - return false; + drivable_area_poly.outer().push_back(drivable_area_poly.outer().front()); + return drivable_area_poly; } } // namespace @@ -668,8 +682,8 @@ namespace drivable_area_utils { bool isOutsideDrivableAreaFromRectangleFootprint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point, - const std::vector left_bound, - const std::vector right_bound, const VehicleParam & vehicle_param) + const std::vector & left_bound, + const std::vector & right_bound, const VehicleParam & vehicle_param) { if (left_bound.empty() || right_bound.empty()) { return false; @@ -694,12 +708,34 @@ bool isOutsideDrivableAreaFromRectangleFootprint( tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0) .position; - const bool out_top_left = isOutsideDrivableArea(top_left_pos, left_bound, right_bound); - const bool out_top_right = isOutsideDrivableArea(top_right_pos, left_bound, right_bound); - const bool out_bottom_left = isOutsideDrivableArea(bottom_left_pos, left_bound, right_bound); - const bool out_bottom_right = isOutsideDrivableArea(bottom_right_pos, left_bound, right_bound); + const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound); + const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound); + const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound); + const bool front_bottom_right = isFrontDrivableArea(bottom_right_pos, left_bound, right_bound); - if (out_top_left || out_top_right || out_bottom_left || out_bottom_right) { + // create rectangle + const auto drivable_area_poly = createDrivablePolygon(left_bound, right_bound); + + if ( + !front_top_left && !bg::within(bg_point(top_left_pos.x, top_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_top_right && + !bg::within(bg_point(top_right_pos.x, top_right_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_left && + !bg::within(bg_point(bottom_left_pos.x, bottom_left_pos.y), drivable_area_poly)) { + return true; + } + + if ( + !front_bottom_right && + !bg::within(bg_point(bottom_right_pos.x, bottom_right_pos.y), drivable_area_poly)) { return true; }