fix(utils): improve monotonic bound generation logic autowarefoundati…

…on#4706 Signed-off-by: kosuke55 <[email protected]>
tier4 · Aug 24, 2023 · fcf8987 · fcf8987
1 parent bd5b24c
commit fcf8987
Show file tree

Hide file tree

Showing 3 changed files with 295 additions and 71 deletions.
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp
@@ -770,6 +770,28 @@ inline bool isTwistCovarianceValid(
   return false;
 }
 
+// NOTE: much faster than boost::geometry::intersects()
+inline std::optional<geometry_msgs::msg::Point> intersect(
+  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
+  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
+{
+  // calculate intersection point
+  const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
+  if (det == 0.0) {
+    return std::nullopt;
+  }
+
+  const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
+  const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
+  if (t < 0 || 1 < t || s < 0 || 1 < s) {
+    return std::nullopt;
+  }
+
+  geometry_msgs::msg::Point intersect_point;
+  intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
+  intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
+  return intersect_point;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -415,7 +415,9 @@ DrivableAreaInfo combineDrivableAreaInfo(
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
 
-void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left);
+void makeBoundLongitudinallyMonotonic(
+  PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
+  const bool is_bound_left);
 
 std::optional<lanelet::Polygon3d> getPolygonByPoint(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,