Merge pull request #420 from tier4/hotfix/behavior-path-change-by-mur…

…ooka

fix(behavior_path_planner): hotfix for behavior path planner

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -121,12 +121,15 @@ void Lanelet2MapVisualizationNode::onMapBin(
   lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out =
     lanelet::utils::query::getAllPolygonsByType(
       viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
+  lanelet::ConstPolygons3d hatched_road_markings_area =
+    lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
     cl_speed_bumps, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons,
     cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
-    cl_no_obstacle_segmentation_area_for_run_out;
+    cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
+    cl_hatched_road_markings_line;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -145,6 +148,8 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5);
   setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
+  setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
+  setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -219,6 +224,11 @@ void Lanelet2MapVisualizationNode::onMapBin(
     lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray(
       no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray(
+      hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line));
+
   pub_marker_->publish(map_marker_array);
 }
 

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -18,6 +18,7 @@
       enable_safety_check: true
       enable_yield_maneuver: true
       disable_path_update: false
+      use_hatched_road_markings: false
 
       # for debug
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp

@@ -89,6 +89,7 @@ struct DrivableAreaInfo
   };
   std::vector<DrivableLanes> drivable_lanes;
   std::vector<Obstacle> obstacles;  // obstacles to extract from the drivable area
+  bool enable_expanding_hatched_road_markings{false};
 
   // temporary only for pull over's freespace planning
   double drivable_margin{0.0};

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -94,6 +94,9 @@ struct AvoidanceParameters
   // disable path update
   bool disable_path_update{false};
 
+  // use hatched road markings for avoidance
+  bool use_hatched_road_markings{false};
+
   // constrains
   bool use_constraints_for_decel{false};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -229,13 +229,18 @@ boost::optional<lanelet::ConstLanelet> getLeftLanelet(
 std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes);
 std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes);
+std::vector<geometry_msgs::msg::Point> calcBound(
+  const std::shared_ptr<RouteHandler> route_handler,
+  const std::vector<DrivableLanes> & drivable_lanes, const bool enable_expanding_polygon,
+  const bool is_left);
 
 boost::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
 std::vector<DrivableLanes> cutOverlappedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
 
 void generateDrivableArea(
-  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
+  const bool enable_expanding_polygon, const double vehicle_length,
   const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
 
 void generateDrivableArea(
@@ -386,8 +391,17 @@ std::vector<DrivableLanes> combineDrivableLanes(
   const std::vector<DrivableLanes> & original_drivable_lanes_vec,
   const std::vector<DrivableLanes> & new_drivable_lanes_vec);
 
+DrivableAreaInfo combineDrivableAreaInfo(
+  const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);
+
 void extractObstaclesFromDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableAreaInfo::Obstacle> & obstacles);
+
+void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound_left);
+
+std::optional<lanelet::Polygon3d> getPolygonByPoint(
+  const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstPoint3d & point,
+  const std::string & polygon_name);
 }  // namespace behavior_path_planner::utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -525,6 +525,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     p.enable_safety_check = declare_parameter<bool>(ns + "enable_safety_check");
     p.enable_yield_maneuver = declare_parameter<bool>(ns + "enable_yield_maneuver");
     p.disable_path_update = declare_parameter<bool>(ns + "disable_path_update");
+    p.use_hatched_road_markings = declare_parameter<bool>(ns + "use_hatched_road_markings");
     p.publish_debug_marker = declare_parameter<bool>(ns + "publish_debug_marker");
     p.print_debug_info = declare_parameter<bool>(ns + "print_debug_info");
   }

planning/behavior_path_planner/src/planner_manager.cpp

@@ -129,23 +129,22 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
 void PlannerManager::generateCombinedDrivableArea(
   BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const
 {
+  const auto & di = output.drivable_area_info;
   constexpr double epsilon = 1e-3;
-  if (epsilon < std::abs(output.drivable_area_info.drivable_margin)) {
+
+  if (epsilon < std::abs(di.drivable_margin)) {
     // for single free space pull over
     const auto is_driving_forward_opt = motion_utils::isDrivingForward(output.path->points);
     const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true;
 
     utils::generateDrivableArea(
-      *output.path, data->parameters.vehicle_length, output.drivable_area_info.drivable_margin,
-      is_driving_forward);
-  } else if (output.drivable_area_info.is_already_expanded) {
+      *output.path, data->parameters.vehicle_length, di.drivable_margin, is_driving_forward);
+  } else if (di.is_already_expanded) {
     // for single side shift
     utils::generateDrivableArea(
-      *output.path, output.drivable_area_info.drivable_lanes, data->parameters.vehicle_length,
-      data);
+      *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data);
   } else {
-    const auto shorten_lanes =
-      utils::cutOverlappedLanes(*output.path, output.drivable_area_info.drivable_lanes);
+    const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes);
 
     const auto & dp = data->drivable_area_expansion_parameters;
     const auto expanded_lanes = utils::expandLanelets(
@@ -154,11 +153,12 @@ void PlannerManager::generateCombinedDrivableArea(
 
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
-      *output.path, expanded_lanes, data->parameters.vehicle_length, data);
+      *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
+      data->parameters.vehicle_length, data);
   }
 
   // extract obstacles from drivable area
-  utils::extractObstaclesFromDrivableArea(*output.path, output.drivable_area_info.obstacles);
+  utils::extractObstaclesFromDrivableArea(*output.path, di.obstacles);
 }
 
 std::vector<SceneModulePtr> PlannerManager::getRequestModules(

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2523,18 +2523,25 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
   }
 
   {  // for new architecture
+    DrivableAreaInfo current_drivable_area_info;
     // generate drivable lanes
-    output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, drivable_lanes);
+    current_drivable_area_info.drivable_lanes = drivable_lanes;
     // generate obstacle polygons
-    output.drivable_area_info.obstacles = utils::avoidance::generateObstaclePolygonsForDrivableArea(
-      avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    current_drivable_area_info.obstacles =
+      utils::avoidance::generateObstaclePolygonsForDrivableArea(
+        avoidance_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0);
+    // expand hatched road markings
+    current_drivable_area_info.enable_expanding_hatched_road_markings =
+      parameters_->use_hatched_road_markings;
+
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   {  // for old architecture
     // NOTE: Obstacles to avoid are not extracted from the drivable area with an old architecture.
     utils::generateDrivableArea(
-      *output.path, drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+      *output.path, drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_);
   }
 }
 

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -639,7 +639,8 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
       utils::clipPathLength(path, ego_idx, planner_data_->parameters);
       const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes);
       utils::generateDrivableArea(
-        path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+        path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length,
+        planner_data_);
     }
   }
 
@@ -691,8 +692,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
   } else {
     const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*output.path, status_.lanes);
-    output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
+
+    DrivableAreaInfo current_drivable_area_info;
+    current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   // return to lane parking if it is possible
@@ -818,8 +822,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
       planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
   } else {
     const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, status_.lanes);
-    out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-      getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
+
+    DrivableAreaInfo current_drivable_area_info;
+    current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+    out.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
   }
 
   return out;
@@ -918,7 +925,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath()
   const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes);
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes);
   utils::generateDrivableArea(
-    reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_);
+    reference_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_);
 
   return reference_path;
 }
@@ -953,7 +960,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath()
   const auto drivable_lanes = utils::generateDrivableLanes(status_.current_lanes);
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(stop_path, drivable_lanes);
   utils::generateDrivableArea(
-    stop_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_);
+    stop_path, target_drivable_lanes, false, common_parameters.vehicle_length, planner_data_);
 
   return stop_path;
 }

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -130,11 +130,18 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
     dp.drivable_area_types_to_skip);
 
   // for new architecture
-  output.drivable_area_info.drivable_lanes = expanded_lanes;
+  DrivableAreaInfo current_drivable_area_info;
+  current_drivable_area_info.drivable_lanes = expanded_lanes;
+  if (prev_drivable_area_info_) {
+    output.drivable_area_info =
+      utils::combineDrivableAreaInfo(current_drivable_area_info, *prev_drivable_area_info_);
+  } else {
+    output.drivable_area_info = current_drivable_area_info;
+  }
 
   // for old architecture
   utils::generateDrivableArea(
-    *output.path, expanded_lanes, common_parameters.vehicle_length, planner_data_);
+    *output.path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_);
 }
 
 bool NormalLaneChange::hasFinishedLaneChange() const
@@ -688,7 +695,7 @@ PathWithLaneId NormalLaneChangeBT::getReferencePath() const
     shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);
   utils::generateDrivableArea(
-    reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_);
+    reference_path, expanded_lanes, false, common_parameters.vehicle_length, planner_data_);
 
   return reference_path;
 }

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -179,7 +179,12 @@ BehaviorModuleOutput PullOutModule::plan()
     // the path of getCurrent() is generated by generateStopPath()
     const PathWithLaneId stop_path = getCurrentPath();
     output.path = std::make_shared<PathWithLaneId>(stop_path);
-    output.drivable_area_info.drivable_lanes = status_.lanes;
+
+    DrivableAreaInfo current_drivable_area_info;
+    current_drivable_area_info.drivable_lanes = status_.lanes;
+    output.drivable_area_info = utils::combineDrivableAreaInfo(
+      current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
+
     output.reference_path = getPreviousModuleOutput().reference_path;
     path_candidate_ = std::make_shared<PathWithLaneId>(stop_path);
     path_reference_ = getPreviousModuleOutput().reference_path;
@@ -199,12 +204,14 @@ BehaviorModuleOutput PullOutModule::plan()
 
   const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, status_.lanes);
   utils::generateDrivableArea(
-    path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
-  output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-    getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes);
+    path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_);
+
+  DrivableAreaInfo current_drivable_area_info;
+  current_drivable_area_info.drivable_lanes = target_drivable_lanes;
+  output.drivable_area_info = utils::combineDrivableAreaInfo(
+    current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
 
   output.path = std::make_shared<PathWithLaneId>(path);
-  output.drivable_area_info.drivable_lanes = status_.lanes;
   output.reference_path = getPreviousModuleOutput().reference_path;
   output.turn_signal_info = calcTurnSignalInfo();
   path_candidate_ = std::make_shared<PathWithLaneId>(getFullPath());
@@ -317,13 +324,15 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
     drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,
     dp.drivable_area_types_to_skip);
   utils::generateDrivableArea(
-    stop_path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+    stop_path, expanded_lanes, false, planner_data_->parameters.vehicle_length, planner_data_);
   for (auto & p : stop_path.points) {
     p.point.longitudinal_velocity_mps = 0.0;
   }
 
-  output.drivable_area_info.drivable_lanes = utils::combineDrivableLanes(
-    getPreviousModuleOutput().drivable_area_info.drivable_lanes, expanded_lanes);
+  DrivableAreaInfo current_drivable_area_info;
+  current_drivable_area_info.drivable_lanes = expanded_lanes;
+  output.drivable_area_info = utils::combineDrivableAreaInfo(
+    current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
 
   output.path = std::make_shared<PathWithLaneId>(stop_path);
   output.drivable_area_info.drivable_lanes = status_.lanes;
@@ -517,7 +526,7 @@ PathWithLaneId PullOutModule::generateStopPath() const
 
   // for old architecture
   utils::generateDrivableArea(
-    path, target_drivable_lanes, planner_data_->parameters.vehicle_length, planner_data_);
+    path, target_drivable_lanes, false, planner_data_->parameters.vehicle_length, planner_data_);
 
   return path;
 }

planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp

@@ -442,7 +442,8 @@ BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & pat
     utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip);
 
   {  // for old architecture
-    utils::generateDrivableArea(output_path, expanded_lanes, p.vehicle_length, planner_data_);
+    utils::generateDrivableArea(
+      output_path, expanded_lanes, false, p.vehicle_length, planner_data_);
     out.path = std::make_shared<PathWithLaneId>(output_path);
   }
 

planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp

@@ -113,7 +113,7 @@ boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
     // NOTE: drivable_area_info is assigned outside this function.
     const auto shorten_lanes = utils::cutOverlappedLanes(shift_path, drivable_lanes);
     utils::generateDrivableArea(
-      shift_path, shorten_lanes, common_parameters.vehicle_length, planner_data_);
+      shift_path, shorten_lanes, false, common_parameters.vehicle_length, planner_data_);
 
     shift_path.header = planner_data_->route_handler->getRouteHeader();