fix(avoidance): check far objects during shifting (#5857)

* fix(avoidance): check far objects during shifting

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* fix(avoidance): update impl

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

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,10 +187,20 @@ class AvoidanceHelper
       return parameters_->object_check_max_forward_distance;
     }
 
+    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(),