solve calc bound problem

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -82,7 +82,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
   const std::vector<DrivableLanes> & drivable_lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool enable_expanding_freespace_areas, const bool is_left,
-  const bool is_driving_forward = true, const bool expand_to_road_edges = false);
+  const bool is_driving_forward = true);
 
 std::vector<geometry_msgs::msg::Point> postProcess(
   const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,

planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -1568,8 +1568,7 @@ std::vector<geometry_msgs::msg::Point> calcBound(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> planner_data,
   const std::vector<DrivableLanes> & drivable_lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
-  const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward,
-  const bool expand_to_road_edges)
+  const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward)
 {
   using motion_utils::removeOverlapPoints;
 
@@ -1620,11 +1619,6 @@ std::vector<geometry_msgs::msg::Point> calcBound(
                  enable_expanding_intersection_areas, is_left, is_driving_forward);
   };
 
-  // If bounds should contain all drivable lanes
-  if (expand_to_road_edges) {
-    return post_process(removeOverlapPoints(to_ros_point(bound_points)), true);
-  }
-
   // Step2. if there is no drivable area defined by polygon, return original drivable bound.
   if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) {
     return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);

planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp

@@ -224,11 +224,13 @@ class SamplingPlannerModule : public SceneModuleInterface
     const double yaw_difference = std::abs(ego_yaw - ref_path_yaw);
 
     // TODO(Daniel) magic numbers
-    constexpr double threshold_lat_distance_for_merging = 0.25;
-    constexpr double threshold_yaw_difference_for_merging = M_PI / 36.0;  // 5 degrees
+    constexpr double threshold_lat_distance_for_merging = 0.15;
+    constexpr double threshold_yaw_difference_for_merging = M_PI / 72.0;  // 2.5 degrees
     const bool merged_back_to_path =
       (std::abs(ego_arc.distance) < threshold_lat_distance_for_merging) &&
       (yaw_difference < threshold_yaw_difference_for_merging);
+    if (isReferencePathSafe() && (merged_back_to_path))
+      for (int i = 0; i < 10; ++i) std::cerr << "MERGED BACK!!!!!\n";
     return isReferencePathSafe() && (merged_back_to_path);
   }
 

planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp

@@ -220,11 +220,9 @@ bool SamplingPlannerModule::isReferencePathSafe() const
   {
     const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
     const auto left_bound = (utils::calcBound(
-      path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
-      true));
+      path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true));
     const auto right_bound = (utils::calcBound(
-      path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
-      true));
+      path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false));
 
     const auto sampling_planner_data =
       createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -445,7 +443,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
     RCLCPP_ERROR(
       rclcpp::get_logger("behavior_path_planner").get_child("utils"),
       "failed to find closest lanelet within route!!!");
-    return {};
+    return getPreviousModuleOutput();
   }
 
   std::vector<DrivableLanes> drivable_lanes{};
@@ -549,11 +547,9 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
 
   const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path;
   const auto left_bound = (utils::calcBound(
-    path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true, true,
-    true));
+    path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, true));
   const auto right_bound = (utils::calcBound(
-    path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false, true,
-    true));
+    path_for_calculating_bounds, planner_data_, drivable_lanes, false, false, false, false));
 
   const auto sampling_planner_data =
     createPlannerData(planner_data_->prev_output_path, left_bound, right_bound);
@@ -630,13 +626,14 @@ BehaviorModuleOutput SamplingPlannerModule::plan()
     BehaviorModuleOutput out;
     PathWithLaneId out_path = (prev_sampling_path_)
                                 ? convertFrenetPathToPathWithLaneID(
-                                    prev_sampling_path_.value(), road_lanes,
+                                    prev_sampling_path_.value(), current_lanes,
                                     planner_data_->route_handler->getGoalPose().position.z)
                                 : PathWithLaneId{};
 
     out.path = (prev_sampling_path_) ? out_path : getPreviousModuleOutput().path;
     out.reference_path = getPreviousModuleOutput().reference_path;
     out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
+    extendOutputDrivableArea(out, drivable_lanes);
     return out;
   }