simplify the functions

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
autowarefoundation · Jan 19, 2023 · a21be1f · a21be1f
1 parent 91568cc
commit a21be1f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 41 deletions.
diff --git a/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
@@ -48,11 +48,6 @@ using tier4_autoware_utils::Polygon2d;
 
 PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2);
 
-bool isPathInLanelets(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
-  const double original_lane_length, const lanelet::ConstLanelets & target_lanelets,
-  const double target_lane_length);
-
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
@@ -61,19 +61,18 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith
 
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
-  const double original_lane_length, const lanelet::ConstLanelets & target_lanelets,
-  const double target_lane_length)
+  const lanelet::ConstLanelets & target_lanelets)
 {
-  const auto current_lane_poly =
-    lanelet::utils::getPolygonFromArcLength(original_lanelets, 0, original_lane_length);
+  const auto current_lane_poly = lanelet::utils::getPolygonFromArcLength(
+    original_lanelets, 0, std::numeric_limits<double>::max());
   const auto target_lane_poly =
-    lanelet::utils::getPolygonFromArcLength(target_lanelets, 0, target_lane_length);
+    lanelet::utils::getPolygonFromArcLength(target_lanelets, 0, std::numeric_limits<double>::max());
   const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon();
   const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon();
   for (const auto & pt : path.points) {
     const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y);
     const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d);
-    if (is_in_current){
+    if (is_in_current) {
       continue;
     }
     const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d);
@@ -84,28 +83,6 @@ bool isPathInLanelets(
   return true;
 }
 
-bool isPathInLanelets(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
-  const lanelet::ConstLanelets & target_lanelets)
-{
-  for (const auto & pt : path.points) {
-    bool is_in_lanelet = false;
-    for (const auto & llt : original_lanelets) {
-      if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) {
-        is_in_lanelet = true;
-      }
-    }
-    for (const auto & llt : target_lanelets) {
-      if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) {
-        is_in_lanelet = true;
-      }
-    }
-    if (!is_in_lanelet) {
-      return false;
-    }
-  }
-  return true;
-}
 double getExpectedVelocityWhenDecelerate(
   const double & velocity, const double & expected_acceleration, const double & duration)
 {
@@ -123,8 +100,7 @@ double getDistanceWhenDecelerate(
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
-  const lanelet::ConstLanelets & original_lanelets, const double original_lane_length,
-  const lanelet::ConstLanelets & target_lanelets, const double target_lane_length,
+  const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
   const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed,
   const LaneChangeParameters & lane_change_param)
 {
@@ -201,9 +177,7 @@ std::optional<LaneChangePath> constructCandidatePath(
   }
 
   // check candidate path is in lanelet
-  if (!isPathInLanelets(
-        candidate_path.path, original_lanelets, original_lane_length, target_lanelets,
-        target_lane_length)) {
+  if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) {
     return std::nullopt;
   }
 
@@ -257,7 +231,6 @@ LaneChangePaths getLaneChangePaths(
   const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose);
   const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose);
 
-  const auto original_lane_length = lanelet::utils::getLaneletLength2d(original_lanelets);
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets);
 
   for (double acceleration = 0.0; acceleration >= -maximum_deceleration;
@@ -320,8 +293,7 @@ LaneChangePaths getLaneChangePaths(
     const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed};
     const auto candidate_path = constructCandidatePath(
       prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path,
-      shift_line, original_lanelets, original_lane_length, target_lanelets, target_lane_length,
-      acceleration, lc_dist, lc_speed, parameter);
+      shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter);
 
     if (!candidate_path) {
       continue;