feat(avoidance): check next & previous road shoulder linestring (auto…

…warefoundation#3691) * feat(route_hander): move public Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): check road next & previous road shoulder Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · Jun 19, 2023 · a84cdf7 · a84cdf7
1 parent 937df16
commit a84cdf7
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 15 deletions.
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
@@ -3300,8 +3300,7 @@ void AvoidanceModule::updateDebugMarker(
   add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold")));
 
   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"));
 

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -1024,21 +1024,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;
+
+      lanelet::ConstLineString3d target_line{};
+      {
+        const auto lines =
+          rh->getFurthestLinestring(overhang_lanelet, get_right, get_left, get_opposite);
+        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());
+        }
+      }
 
-      const auto target_lines = rh->getFurthestLinestring(
-        overhang_lanelet, get_right, get_left,
-        parameters->enable_avoidance_over_opposite_direction);
+      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);
+        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());
+        }
+      }
 
-      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::ConstLanelet next_lanelet{};
+      if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) {
+        const auto lines =
+          rh->getFurthestLinestring(next_lanelet, get_right, get_left, get_opposite);
+        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());
+        }
       }
     }
 

diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/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;
@@ -379,8 +381,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(