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 c0bd621cc86b4..d012fd5f612fb 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 @@ -164,7 +164,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); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, 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 632aad8873906..8edc291fd560d 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 @@ -292,12 +292,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 0e7eb87feed9b..ed2e42bf0c5ff 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -29,9 +29,12 @@ #include +#include #include #include #include +#include +#include #include #include @@ -1602,7 +1605,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; @@ -1639,6 +1642,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;