diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 00793532023b9..943d884acab8a 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -369,10 +369,12 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); + const auto factor_status = module_type_->isAbortState() ? uint16_t(100) : SteeringFactor::TURNING; + steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); + factor_status, ""); } void LaneChangeInterface::updateSteeringFactorPtr( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 7ceafb6f428de..3ccc92d04025f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -286,6 +286,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishSteeringFactor(); + module_ptr->publishObjectsOfInterestMarker(); processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true);