po

tier4 · Apr 24, 2023 · 8d4cded · 8d4cded
1 parent e993c8b
commit 8d4cded
Show file tree

Hide file tree

Showing 6 changed files with 423 additions and 423 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/util.hpp
@@ -26,30 +26,6 @@ namespace behavior_path_planner
 {
 using behavior_path_planner::PlannerData;
 
-struct PolygonPoint
-{
-  geometry_msgs::msg::Point point;
-  size_t bound_seg_idx;
-  double lon_dist_to_segment;
-  double lat_dist_to_bound;
-
-  bool is_after(const PolygonPoint & other_point) const
-  {
-    if (bound_seg_idx == other_point.bound_seg_idx) {
-      return other_point.lon_dist_to_segment < lon_dist_to_segment;
-    }
-    return other_point.bound_seg_idx < bound_seg_idx;
-  }
-
-  bool is_outside_bounds(const bool is_on_right) const
-  {
-    if (is_on_right) {
-      return lat_dist_to_bound < 0.0;
-    }
-    return 0.0 < lat_dist_to_bound;
-  };
-};
-
 bool isOnRight(const ObjectData & obj);
 
 bool isTargetObjectType(

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
@@ -77,6 +77,30 @@ using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::Polygon2d;
 using vehicle_info_util::VehicleInfo;
 
+struct PolygonPoint
+{
+  geometry_msgs::msg::Point point;
+  size_t bound_seg_idx;
+  double lon_dist_to_segment;
+  double lat_dist_to_bound;
+
+  bool is_after(const PolygonPoint & other_point) const
+  {
+    if (bound_seg_idx == other_point.bound_seg_idx) {
+      return other_point.lon_dist_to_segment < lon_dist_to_segment;
+    }
+    return other_point.bound_seg_idx < bound_seg_idx;
+  }
+
+  bool is_outside_bounds(const bool is_on_right) const
+  {
+    if (is_on_right) {
+      return lat_dist_to_bound < 0.0;
+    }
+    return 0.0 < lat_dist_to_bound;
+  };
+};
+
 struct FrenetPoint
 {
   double length{0.0};    // longitudinal

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -2513,11 +2513,6 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
     drivable_lanes.push_back(current_drivable_lanes);
   }
 
-  {  // for old architecture
-    utils::generateDrivableArea(
-      *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
-  }
-
   {  // for new architecture
     // generate drivable lanes
     output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
@@ -2526,6 +2521,13 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
     output.drivable_area_info.obstacle_polys = generateObstaclePolygonsForDrivableArea(
       avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
   }
+
+  {  // for old architecture
+    utils::generateDrivableArea(
+      *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+
+    // TODO(murooka) remove obstacles from drivable area
+  }
 }
 
 void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & path)

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -124,7 +124,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
     resetPathIfAbort();
   }
 
-  module_type_->setPreviousDrivableLanes(getPreviousModuleOutput().drivable_lanes);
+  module_type_->setPreviousDrivableLanes(
+    getPreviousModuleOutput().drivable_area_info.drivable_lanes);
   auto output = module_type_->generateOutput();
   path_reference_ = output.reference_path;
   *prev_approved_path_ = *getPreviousModuleOutput().path;