fix(behavior_path_planner_common): drivable are bug

common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp

@@ -80,8 +80,12 @@
       if (diff_angle <= 1e-7 && diff_angle >= -1e-7) {
         return std::make_pair(normal_vector_angle, width);
       }
-
-      return std::make_pair(normal_vector_angle, width / std::sin(diff_angle));
+      const float adjusted_width = width / std::sin(diff_angle);
+      if (std::abs(adjusted_width) > 1.0) {
+        return std::make_pair(normal_vector_angle, adjusted_width / std::abs(adjusted_width));
+      } else {
+        return std::make_pair(normal_vector_angle, adjusted_width);
+      }
     }();
 
     normal_vector_angles.push_back(normal_vector_angle);

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -207,8 +207,7 @@ void PlannerManager::generateCombinedDrivableArea(
       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, di.drivable_lanes, false, false, false, data, is_driving_forward);
+    utils::generateDrivableArea(output.path, di.drivable_lanes, false, false, false, data);
   } else {
     const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes);
 
@@ -220,8 +219,7 @@ void PlannerManager::generateCombinedDrivableArea(
     // for other modules where multiple modules may be launched
     utils::generateDrivableArea(
       output.path, expanded_lanes, di.enable_expanding_hatched_road_markings,
-      di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data,
-      is_driving_forward);
+      di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data);
   }
 
   // extract obstacles from drivable area

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp

@@ -42,7 +42,7 @@ void generateDrivableArea(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool enable_expanding_freespace_areas,
-  const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true);
+  const std::shared_ptr<const PlannerData> planner_data);
 
 void generateDrivableArea(
   PathWithLaneId & path, const double vehicle_length, const double offset,
@@ -81,14 +81,12 @@ 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 = true);
+  const bool enable_expanding_freespace_areas, const bool is_left);
 
 std::vector<geometry_msgs::msg::Point> postProcess(
   const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> planner_data,
-  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left,
-  const bool is_driving_forward = true);
+  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left);
 
 DrivableAreaInfo combineDrivableAreaInfo(
   const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2);

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -800,24 +800,22 @@
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes,
   const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas,
   const bool enable_expanding_freespace_areas,
-  const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward)
+  const std::shared_ptr<const PlannerData> planner_data)
 {
   if (path.points.empty()) {
     return;
   }
 
   path.left_bound.clear();
   path.right_bound.clear();
 
   // Insert Position
   path.left_bound = calcBound(
     path, planner_data, lanes, enable_expanding_hatched_road_markings,
-    enable_expanding_intersection_areas, enable_expanding_freespace_areas, true,
-    is_driving_forward);
+    enable_expanding_intersection_areas, enable_expanding_freespace_areas, true);
   path.right_bound = calcBound(
     path, planner_data, lanes, enable_expanding_hatched_road_markings,
-    enable_expanding_intersection_areas, enable_expanding_freespace_areas, false,
-    is_driving_forward);
+    enable_expanding_intersection_areas, enable_expanding_freespace_areas, false);
 
   if (path.left_bound.empty() || path.right_bound.empty()) {
     auto clock{rclcpp::Clock{RCL_ROS_TIME}};
@@ -1459,128 +1457,127 @@
 std::vector<geometry_msgs::msg::Point> postProcess(
   const std::vector<geometry_msgs::msg::Point> & original_bound, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> planner_data,
-  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left,
-  const bool is_driving_forward)
+  const std::vector<DrivableLanes> & drivable_lanes, const bool is_left)
 {
   const auto lanelets = utils::transformToLanelets(drivable_lanes);
   const auto & current_pose = planner_data->self_odometry->pose.pose;
   const auto & route_handler = planner_data->route_handler;
   const auto & vehicle_length = planner_data->parameters.vehicle_length;
   constexpr double overlap_threshold = 0.01;
 
   if (original_bound.size() < 2) {
     return original_bound;
   }
 
   const auto addPoints =
     [](const lanelet::ConstLineString3d & points, std::vector<geometry_msgs::msg::Point> & bound) {
       for (const auto & bound_p : points) {
         const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p);
         if (bound.empty()) {
           bound.push_back(cp);
         } else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) {
           bound.push_back(cp);
         }
       }
     };
 
   const auto has_overlap =
     [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) {
       for (const auto & transformed_lane : lanelets) {
         if (checkHasSameLane(ignore_lanelets, transformed_lane)) {
           continue;
         }
         if (boost::geometry::intersects(
               lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) {
           return true;
         }
       }
       return false;
     };
 
   std::vector<geometry_msgs::msg::Point> processed_bound;
   std::vector<geometry_msgs::msg::Point> tmp_bound = original_bound;
 
   // Insert points after goal
   lanelet::ConstLanelet goal_lanelet;
   if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) {
     const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length);
     const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet);
     const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet);
     const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet);
     lanelet::ConstLanelets goal_lanelets = {goal_lanelet};
     if (goal_left_lanelet.has_value()) {
       goal_lanelets.push_back(goal_left_lanelet.value());
     }
     if (goal_right_lanelet.has_value()) {
       goal_lanelets.push_back(goal_right_lanelet.value());
     }
 
     for (const auto & lane : lanes_after_goal) {
       // If lane is already in the transformed lanes, ignore it
       if (checkHasSameLane(lanelets, lane)) {
         continue;
       }
       // Check if overlapped
       const bool is_overlapped =
         (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets)
                                                        : has_overlap(lane));
       if (is_overlapped) {
         continue;
       }
 
       if (is_left) {
         addPoints(lane.leftBound3d(), tmp_bound);
       } else {
         addPoints(lane.rightBound3d(), tmp_bound);
       }
     }
   }
 
-  if (!is_driving_forward) {
-    std::reverse(tmp_bound.begin(), tmp_bound.end());
-  }
-
   const auto start_idx = [&]() {
     const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points);
     const auto cropped_path_points = autoware::motion_utils::cropPoints(
       path.points, current_pose.position, current_seg_idx,
       planner_data->parameters.forward_path_length,
       planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval);
 
     constexpr double front_length = 0.5;
     const auto front_pose =
       cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose;
     const size_t front_start_idx =
       findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2);
     const auto start_point =
       calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length);
 
     // Insert a start point
     processed_bound.push_back(start_point);
 
     const auto p_tmp =
       geometry_msgs::build<Pose>().position(start_point).orientation(front_pose.orientation);
     return findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2);
   }();
 
   // Get Closest segment for the goal point
   const auto [goal_idx, goal_point] = [&]() {
     const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose;
     const size_t goal_start_idx =
       findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2);
     const auto goal_point =
       calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length);
     const auto p_tmp =
       geometry_msgs::build<Pose>().position(goal_point).orientation(goal_pose.orientation);
-    const size_t goal_idx = std::max(
-      goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2));
-
+    const size_t goal_nearest_idx =
+      findNearestSegmentIndexFromLateralDistance(tmp_bound, p_tmp, M_PI_2);
+    const size_t goal_idx = ((goal_start_idx - start_idx) * (goal_start_idx - start_idx) >
+                             (goal_nearest_idx - start_idx) * (goal_nearest_idx - start_idx))
+                              ? goal_start_idx
+                              : goal_nearest_idx;
     return std::make_pair(goal_idx, goal_point);
   }();
 
   // Insert middle points
-  for (size_t i = start_idx + 1; i <= goal_idx; ++i) {
+  size_t step = (start_idx < goal_idx) ? 1 : -1;
+  for (size_t i = start_idx + step; i != goal_idx + step; i += step) {
     const auto & next_point = tmp_bound.at(i);
     const double dist =
       autoware::universe_utils::calcDistance2d(processed_bound.back(), next_point);
@@ -1604,53 +1601,51 @@
   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 enable_expanding_freespace_areas, const bool is_left)
 {
   using autoware::motion_utils::removeOverlapPoints;
 
   const auto & route_handler = planner_data->route_handler;
 
   // a function to convert drivable lanes to points without duplicated points
   const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) {
     constexpr double overlap_threshold = 0.01;
 
     std::vector<lanelet::ConstPoint3d> points;
     for (const auto & drivable_lane : drivable_lanes) {
       const auto bound =
         is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d();
       for (const auto & point : bound) {
         if (
           points.empty() ||
           overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) {
           points.push_back(point);
         }
       }
     }
     return points;
   };
 
   const auto to_ros_point = [](const std::vector<lanelet::ConstPoint3d> & bound) {
     std::vector<Point> ret{};
     std::for_each(bound.begin(), bound.end(), [&](const auto & p) {
       ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p));
     });
     return ret;
   };
 
   // Step1. create drivable bound from drivable lanes.
   auto [bound_points, skip_post_process] = [&]() {
     if (!enable_expanding_freespace_areas) {
       return std::make_pair(convert_to_points(drivable_lanes, is_left), false);
     }
     return getBoundWithFreeSpaceAreas(
       convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left),
       planner_data, is_left);
   }();
 
   const auto post_process = [&](const auto & bound, const auto skip) {
-    return skip
-             ? bound
-             : postProcess(bound, path, planner_data, drivable_lanes, is_left, is_driving_forward);
+    return skip ? bound : postProcess(bound, path, planner_data, drivable_lanes, is_left);
   };
 
   // Step2. if there is no drivable area defined by polygon, return original drivable bound.