Merge pull request autowarefoundation#550 from tier4/hotfix/behavior-…

…path

fix(avoidance): hotfix

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -185,6 +185,7 @@ class AvoidanceModule : public SceneModuleInterface
   ShiftedPath prev_output_;
   ShiftedPath prev_linear_shift_path_;  // used for shift point check
   PathWithLaneId prev_reference_;
+  lanelet::ConstLanelets prev_driving_lanes_;
 
   // for raw_shift_line registration
   AvoidLineArray registered_raw_shift_lines_;

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

@@ -76,6 +76,25 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
 {
   base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end());
 }
+
+bool isDrivingSameLane(
+  const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes)
+{
+  std::multiset<lanelet::Id> prev_ids{};
+  std::multiset<lanelet::Id> curr_ids{};
+  std::multiset<lanelet::Id> same_ids{};
+
+  std::for_each(
+    previous_lanes.begin(), previous_lanes.end(), [&](const auto & l) { prev_ids.insert(l.id()); });
+  std::for_each(
+    current_lanes.begin(), current_lanes.end(), [&](const auto & l) { curr_ids.insert(l.id()); });
+
+  std::set_intersection(
+    prev_ids.begin(), prev_ids.end(), curr_ids.begin(), curr_ids.end(),
+    std::inserter(same_ids, same_ids.end()));
+
+  return !same_ids.empty();
+}
 }  // namespace
 
 #ifdef USE_OLD_ARCHITECTURE
@@ -164,9 +183,15 @@ ModuleStatus AvoidanceModule::updateState()
     current_state_ = ModuleStatus::RUNNING;
   }
 
+  if (!isDrivingSameLane(prev_driving_lanes_, avoidance_data_.current_lanelets)) {
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated.");
+    current_state_ = ModuleStatus::SUCCESS;
+  }
+
   DEBUG_PRINT(
     "is_plan_running = %d, has_avoidance_target = %d", is_plan_running, has_avoidance_target);
 
+  prev_driving_lanes_ = avoidance_data_.current_lanelets;
   return current_state_;
 }
 
@@ -3125,6 +3150,10 @@ void AvoidanceModule::updateData()
   if (prev_reference_.points.empty()) {
     prev_reference_ = *getPreviousModuleOutput().path;
   }
+  if (prev_driving_lanes_.empty()) {
+    prev_driving_lanes_ =
+      utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_);
+  }
 #endif
 
   debug_data_ = DebugData();
@@ -3181,6 +3210,7 @@ void AvoidanceModule::initVariables()
   prev_output_ = ShiftedPath();
   prev_linear_shift_path_ = ShiftedPath();
   prev_reference_ = PathWithLaneId();
+  prev_driving_lanes_.clear();
   path_shifter_ = PathShifter{};
 
   debug_data_ = DebugData();
@@ -3366,8 +3396,7 @@ void AvoidanceModule::updateDebugMarker(
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance")));
 
   add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang"));
-  add(createOverhangFurthestLineStringMarkerArray(
-    debug.bounds, "farthest_linestring_from_overhang", 1.0, 0.0, 1.0));
+  add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0));
 
   add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects"));
 

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

@@ -103,6 +103,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
   output.turn_signal_info = updateOutputTurnSignal();
 
   if (isAbortState()) {
+    output.reference_path = std::make_shared<PathWithLaneId>(prev_module_reference_path_);
     return output;
   }
 

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -675,21 +675,58 @@ void filterTargetObjects(
       o.overhang_lanelet = overhang_lanelet;
       lanelet::BasicPoint3d overhang_basic_pose(
         o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z);
+
       const bool get_left = isOnRight(o) && parameters->enable_avoidance_over_same_direction;
       const bool get_right = !isOnRight(o) && parameters->enable_avoidance_over_same_direction;
+      const bool get_opposite = parameters->enable_avoidance_over_opposite_direction;
 
-      const auto target_lines = rh->getFurthestLinestring(
-        overhang_lanelet, get_right, get_left,
-        parameters->enable_avoidance_over_opposite_direction);
+      lanelet::ConstLineString3d target_line{};
+      {
+        const auto lines =
+          rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite, true);
+        if (isOnRight(o)) {
+          o.to_road_shoulder_distance =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
+          debug.bounds.push_back(lines.back());
+        } else {
+          o.to_road_shoulder_distance =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString()));
+          debug.bounds.push_back(lines.front());
+        }
+      }
 
-      if (isOnRight(o)) {
-        o.to_road_shoulder_distance =
-          distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString()));
-        debug.bounds.push_back(target_lines.back());
-      } else {
-        o.to_road_shoulder_distance =
-          distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString()));
-        debug.bounds.push_back(target_lines.front());
+      lanelet::ConstLanelets previous_lanelet{};
+      if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) {
+        const auto lines = rh->getFurthestLinestring(
+          previous_lanelet.front(), get_right, get_left, get_opposite, true);
+        if (isOnRight(o)) {
+          const auto d =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
+          o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance);
+          debug.bounds.push_back(lines.back());
+        } else {
+          const auto d =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString()));
+          o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance);
+          debug.bounds.push_back(lines.front());
+        }
+      }
+
+      lanelet::ConstLanelet next_lanelet{};
+      if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) {
+        const auto lines =
+          rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite, true);
+        if (isOnRight(o)) {
+          const auto d =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.back().basicLineString()));
+          o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance);
+          debug.bounds.push_back(lines.back());
+        } else {
+          const auto d =
+            distance2d(to2D(overhang_basic_pose), to2D(lines.front().basicLineString()));
+          o.to_road_shoulder_distance = std::min(d, o.to_road_shoulder_distance);
+          debug.bounds.push_back(lines.front());
+        }
       }
     }
 

planning/route_handler/include/route_handler/route_handler.hpp

@@ -105,6 +105,8 @@ class RouteHandler
   // for lanelet
   bool getPreviousLaneletsWithinRoute(
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelets * prev_lanelets) const;
+  bool getNextLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
   bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getLaneletsFromPoint(const lanelet::ConstPoint3d & point) const;
   lanelet::ConstLanelets getLaneChangeableNeighbors(const lanelet::ConstLanelet & lanelet) const;
@@ -117,7 +119,7 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   boost::optional<lanelet::ConstLanelet> getRightLanelet(
-    const lanelet::ConstLanelet & lanelet) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
 
   /**
    * @brief Check if same-direction lane is available at the left side of the lanelet
@@ -127,7 +129,7 @@ class RouteHandler
    * @return vector of lanelet having same direction if true
    */
   boost::optional<lanelet::ConstLanelet> getLeftLanelet(
-    const lanelet::ConstLanelet & lanelet) const;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
   lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const;
   lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const;
 
@@ -189,7 +191,8 @@ class RouteHandler
    * @param the lanelet of interest
    * @return vector of lanelet having same direction if true
    */
-  lanelet::ConstLanelet getMostRightLanelet(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelet getMostRightLanelet(
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
 
   /**
    * @brief Check if same-direction lane is available at the left side of the lanelet
@@ -198,7 +201,8 @@ class RouteHandler
    * @param the lanelet of interest
    * @return vector of lanelet having same direction if true
    */
-  lanelet::ConstLanelet getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelet getMostLeftLanelet(
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const;
 
   /**
    * @brief Searches the furthest linestring to the right side of the lanelet
@@ -207,7 +211,7 @@ class RouteHandler
    * @return right most linestring of the lane with same direction
    */
   lanelet::ConstLineString3d getRightMostSameDirectionLinestring(
-    const lanelet::ConstLanelet & lanelet) const noexcept;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;
 
   /**
    * @brief Searches the furthest linestring to the right side of the lanelet
@@ -216,7 +220,7 @@ class RouteHandler
    * @return right most linestring
    */
   lanelet::ConstLineString3d getRightMostLinestring(
-    const lanelet::ConstLanelet & lanelet) const noexcept;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;
 
   /**
    * @brief Searches the furthest linestring to the left side of the lanelet
@@ -225,7 +229,7 @@ class RouteHandler
    * @return left most linestring of the lane with same direction
    */
   lanelet::ConstLineString3d getLeftMostSameDirectionLinestring(
-    const lanelet::ConstLanelet & lanelet) const noexcept;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;
 
   /**
    * @brief Searches the furthest linestring to the left side of the lanelet
@@ -234,7 +238,7 @@ class RouteHandler
    * @return left most linestring
    */
   lanelet::ConstLineString3d getLeftMostLinestring(
-    const lanelet::ConstLanelet & lanelet) const noexcept;
+    const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept;
 
   /**
    * @brief Return furthest linestring on both side of the lanelet
@@ -246,7 +250,7 @@ class RouteHandler
    */
   lanelet::ConstLineStrings3d getFurthestLinestring(
     const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true,
-    bool is_opposite = true) const noexcept;
+    bool is_opposite = true, bool enable_same_root = false) const noexcept;
 
   /**
    * Retrieves a sequence of lanelets before the given lanelet.
@@ -379,8 +383,6 @@ class RouteHandler
   bool isBijectiveConnection(
     const lanelet::ConstLanelets & lanelet_section1,
     const lanelet::ConstLanelets & lanelet_section2) const;
-  bool getNextLaneletWithinRoute(
-    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
   bool getPreviousLaneletWithinRouteExceptGoal(
     const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
   bool getNextLaneletWithinRouteExceptStart(