From 7898912a6d135efeceb5f4b102bee908da667857 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 13:08:38 +0900 Subject: [PATCH 1/2] fix(avoidance): align execution request condition (#5266) Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 index 3fd8760946c7e..d7427352cb05d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -129,7 +129,10 @@ bool AvoidanceModule::isExecutionRequested() const return false; } - return !avoid_data_.target_objects.empty(); + return std::any_of( + avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); } bool AvoidanceModule::isExecutionReady() const From cc3e8ca8c288dc619fc9aa6f02a05b23bcc317a5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 14:01:36 +0900 Subject: [PATCH 2/2] fix(avoidance): expand detection area to prevent chattering (#5267) Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 2 +- .../avoidance/avoidance_module.cpp | 6 +++- .../src/utils/avoidance/utils.cpp | 31 ++++++++++++++++++- 3 files changed, 36 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0dc07f0950716..3afb9099a65ef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -166,7 +166,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug); + const bool is_running, DebugData & debug); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ 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 index d7427352cb05d..3c5b0ae55ad7e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -250,12 +250,16 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; + // Add margin in order to prevent avoidance request chattering only when the module is running. + const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || + getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( utils::resamplePathWithSpline( helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output), - planner_data_, data, parameters_, debug); + planner_data_, data, parameters_, is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b60abbddf6632..e744ae3729c48 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -24,6 +24,16 @@ #include +#include +#include +#include +#include +#include +#include +#include + +#include + #include #include #include @@ -1620,7 +1630,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug) + const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1657,6 +1667,25 @@ std::pair separateObjectsByPath( } } + // expand detection area width only when the module is running. + if (is_running) { + constexpr int PER_CIRCLE = 36; + constexpr double MARGIN = 1.0; // [m] + boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); + boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::model::multi_polygon result; + // Create the buffer of a multi polygon + boost::geometry::buffer( + attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + if (!result.empty()) { + attention_area = result.front(); + } + } + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); const auto objects = planner_data->dynamic_object->objects;