fix(avoidance): unexpected road shoulder distance calculation result (#6911)

satoshi-ota · web-flow · commit 8265f9383e5e · 2024-05-16T10:14:36.000Z
* fix(avoidance): unexpected road shoulder distance calculation result

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;

* fix(avoidance): prevent division by zero

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;

---------

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -912,8 +912,14 @@ double getRoadShoulderDistance(
         }
       }
 
+      const auto envelope_polygon_width =
+        boost::geometry::area(object.envelope_poly) /
+        std::max(object.length, 1e-3);  // prevent division by zero
+
       {
-        const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position;
+        const auto p2 =
+          calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0)
+            .position;
         const auto opt_intersect =
           tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));
 

Original file line number	Diff line number	Diff line change
`@@ -912,8 +912,14 @@ double getRoadShoulderDistance(`
`912`	`912`	`}`
`913`	`913`	`}`
`914`	`914`
	`915`	`+ const auto envelope_polygon_width =`
	`916`	`+ boost::geometry::area(object.envelope_poly) /`
	`917`	`+ std::max(object.length, 1e-3); // prevent division by zero`
	`918`	`+`
`915`	`919`	`{`
`916`		`- const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position;`
	`920`	`+ const auto p2 =`
	`921`	`+ calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0)`
	`922`	`+ .position;`
`917`	`923`	`const auto opt_intersect =`
`918`	`924`	`tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i));`
`919`	`925`