From 171b4a1e140f9d27bc232f5b8a8c4066a4aa24f2 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Wed, 13 Dec 2023 14:40:12 +0900 Subject: [PATCH 1/2] fix(avoidance): check far objects during shifting Signed-off-by: satoshi-ota --- .../include/behavior_path_avoidance_module/helper.hpp | 4 ++++ 1 file changed, 4 insertions(+) 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 index 6717cefefc9f6..e552e3ea9afcf 100644 --- 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 @@ -185,6 +185,10 @@ class AvoidanceHelper return parameters_->object_check_max_forward_distance; } + if (isShifted()) { + return parameters_->object_check_max_forward_distance; + } + 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( From 0f47c1e8b932f2cd9945907159a78893c30b2bcf Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 15 Dec 2023 18:06:43 +0900 Subject: [PATCH 2/2] fix(avoidance): update impl Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 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 index e552e3ea9afcf..df9640fa75626 100644 --- 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 & 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(),