Skip to content

Commit

Permalink
refactor(lane_change): use common logger (autowarefoundation#4072)
Browse files Browse the repository at this point in the history 
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe @rej55
TakaHoribe authored and rej55 committed Jun 29, 2023
1 parent 066977c commit e28d0da
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 44 deletions.
2 changes: 2 additions & 0 deletions ...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
58 changes: 14 additions & 44 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -521,19 +521,15 @@ 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;
}

auto prepare_segment = getPrepareSegment(
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;
}

Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}
}
Expand All @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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;
}

Expand All @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand All @@ -943,19 +922,15 @@ 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;
}

ShiftedPath shifted_path;
// 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(
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit e28d0da

Please sign in to comment.