From 6dc708ce70c055a4fa018d5a199e4df99bc384ee Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 22 Jun 2023 13:35:41 +0900 Subject: [PATCH] fix(behavior_path_planner): skip unnecessary process in updateDate() when module_status is not RUNNING (#4031) * update Signed-off-by: kyoichi-sugahara * skip unnecessary process in updateDate() when module_status is not RUNNING Signed-off-by: kyoichi-sugahara * skip unnecessary process in updateDate() when module_status is not RUNNING Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../side_shift/side_shift_module.cpp | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index f1f2cfa44a952..fe2a03460b013 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -186,6 +186,22 @@ ModuleStatus SideShiftModule::updateState() void SideShiftModule::updateData() { +#ifndef USE_OLD_ARCHITECTURE + if ( + planner_data_->lateral_offset != nullptr && + planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) { + if (isReadyForNextRequest(parameters_->shift_request_time_limit)) { + lateral_offset_change_request_ = true; + requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset; + latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp; + } + } +#endif + + if (current_state_ != ModuleStatus::RUNNING) { + return; + } + // special for avoidance: take behind distance upt ot shift-start-point if it exist. const auto longest_dist_to_shift_line = [&]() { double max_dist = 0.0; @@ -238,18 +254,6 @@ void SideShiftModule::updateData() const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); - -#ifndef USE_OLD_ARCHITECTURE - if ( - planner_data_->lateral_offset != nullptr && - planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) { - if (isReadyForNextRequest(parameters_->shift_request_time_limit)) { - lateral_offset_change_request_ = true; - requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset; - latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp; - } - } -#endif } void SideShiftModule::replaceShiftLine()