diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 5119a5f22d6b1..8173237cbc22a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -107,6 +107,8 @@ class NormalLaneChange : public LaneChangeBase void calcTurnSignalInfo() override; bool isValidPath(const PathWithLaneId & path) const override; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; class NormalLaneChangeBT : public NormalLaneChange diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 0ce4659d4a6dc..a97d8a2b25e43 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -521,9 +521,7 @@ bool NormalLaneChange::getLaneChangePaths( minimum_prepare_length); if (prepare_length < target_length) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare length is shorter than distance to target lane!!"); + RCLCPP_DEBUG(logger_, "prepare length is shorter than distance to target lane!!"); break; } @@ -531,9 +529,7 @@ bool NormalLaneChange::getLaneChangePaths( original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare segment is empty!!"); + RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); continue; } @@ -547,8 +543,7 @@ bool NormalLaneChange::getLaneChangePaths( // that case, the lane change shouldn't be executed. if (target_length_from_lane_change_start_pose > 0.0) { RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); break; } @@ -581,9 +576,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "length of lane changing path is longer than length to goal!!"); + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } @@ -596,9 +589,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + common_parameter.lane_change_finish_judge_buffer + next_lane_change_buffer > s_goal) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "length of lane changing path is longer than length to goal!!"); + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } } @@ -609,9 +600,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target segment is empty!! something wrong..."); + RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); continue; } @@ -633,9 +622,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target_lane_reference_path is empty!!"); + RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); continue; } @@ -653,9 +640,7 @@ bool NormalLaneChange::getLaneChangePaths( lc_velocity, terminal_lane_changing_velocity, lc_time); if (!candidate_path) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "no candidate path!!"); + RCLCPP_DEBUG(logger_, "no candidate path!!"); continue; } @@ -664,9 +649,7 @@ bool NormalLaneChange::getLaneChangePaths( minimum_lane_changing_velocity, common_parameter, direction); if (!is_valid) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "invalid candidate path!!"); + RCLCPP_DEBUG(logger_, "invalid candidate path!!"); continue; } @@ -905,18 +888,14 @@ bool NormalLaneChange::getAbortPath() lane_change_parameters_->abort_delta_time + lane_change_parameters_->aborting_time); if (abort_start_idx >= abort_return_idx) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "abort start idx and return idx is equal. can't compute abort path."); + RCLCPP_ERROR(logger_, "abort start idx and return idx is equal. can't compute abort path."); return false; } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, direction)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "insufficient distance to abort."); + RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -943,9 +922,7 @@ bool NormalLaneChange::getAbortPath() path_shifter.setLateralAccelerationLimit(max_lateral_acc); if (lateral_jerk > lane_change_parameters_->abort_max_lateral_jerk) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "Aborting jerk is too strong. lateral_jerk = " << lateral_jerk); + RCLCPP_ERROR(logger_, "Aborting jerk is too strong. lateral_jerk = %f", lateral_jerk); return false; } @@ -953,9 +930,7 @@ bool NormalLaneChange::getAbortPath() // offset front side // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "failed to generate abort shifted path."); + RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } const auto arc_position = lanelet::utils::getArcCoordinates( @@ -1072,12 +1047,7 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment( const double s_start = arc_length_from_current - backward_path_length; const double s_end = arc_length_from_current + prepare_length; - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); + RCLCPP_DEBUG(logger_, "start: %f, end: %f", s_start, s_end); PathWithLaneId prepare_segment = getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end);