fix(avoidance): update impl

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Dec 15, 2023 · 0f47c1e · 0f47c1e
1 parent 171b4a1
commit 0f47c1e
Showing 1 changed file with 11 additions and 3 deletions.
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp
@@ -58,6 +58,8 @@ class AvoidanceHelper
 
   double getEgoSpeed() const { return std::abs(data_->self_odometry->twist.twist.linear.x); }
 
+  geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; }
+
   size_t getConstraintsMapIndex(const double velocity, const std::vector<double> & values) const
   {
     const auto itr = std::find_if(
@@ -185,14 +187,20 @@ class AvoidanceHelper
       return parameters_->object_check_max_forward_distance;
     }
 
-    if (isShifted()) {
+    const auto & route_handler = data_->route_handler;
+
+    lanelet::ConstLanelet closest_lane;
+    if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &closest_lane)) {
       return parameters_->object_check_max_forward_distance;
     }
 
+    const auto limit = route_handler->getTrafficRulesPtr()->speedLimit(closest_lane);
+    const auto speed = isShifted() ? limit.speedLimit.value() : getEgoSpeed();
+
     const auto max_shift_length = std::max(
       std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length));
-    const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk(
-      max_shift_length, getLateralMinJerkLimit(), getEgoSpeed());
+    const auto dynamic_distance =
+      PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed);
 
     return std::clamp(
       1.5 * dynamic_distance + getNominalPrepareDistance(),