Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(lane_change): use common logger #4072

Merged
merged 1 commit into from
Jun 25, 2023
Merged

refactor(lane_change): use common logger #4072

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
refactor(lane_change): use common logger 
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe
TakaHoribe committed Jun 25, 2023
commit 48b825f2aca8adf400f0b39216acd39f3a5b4106
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
@@ -107,6 +107,8 @@ class NormalLaneChange : public LaneChangeBase
TurnSignalInfo calcTurnSignalInfo() override;

bool isValidPath(const PathWithLaneId & path) const override;

rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
};

class NormalLaneChangeBT : public NormalLaneChange
55 changes: 14 additions & 41 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Original file line number Diff line number Diff line change
@@ -561,19 +561,15 @@ bool NormalLaneChange::getLaneChangePaths(
minimum_prepare_length);

if (prepare_length < target_length) {
RCLCPP_DEBUG(
rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()),
"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("lane_change").get_child(getModuleTypeStr()),
"prepare segment is empty!!");
RCLCPP_DEBUG(logger_, "prepare segment is empty!!");
continue;
}

@@ -587,8 +583,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("lane_change").get_child(getModuleTypeStr()),
"[only new arch] lane change start getEgoPose() is behind target lanelet!!");
logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!");
break;
}

@@ -621,9 +616,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("lane_change").get_child(getModuleTypeStr()),
"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;
}

@@ -636,9 +629,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;
}
}
@@ -649,9 +640,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;
}

@@ -673,9 +662,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;
}

@@ -693,9 +680,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;
}

@@ -704,9 +689,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;
}

@@ -927,18 +910,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("lane_change").get_child(getModuleTypeStr()),
"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("lane_change").get_child(getModuleTypeStr()),
"insufficient distance to abort.");
RCLCPP_ERROR(logger_, "insufficient distance to abort.");
return false;
}

@@ -965,19 +944,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("lane_change").get_child(getModuleTypeStr()),
"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("lane_change").get_child(getModuleTypeStr()),
"failed to generate abort shifted path.");
RCLCPP_ERROR(logger_, "failed to generate abort shifted path.");
}

const auto arc_position = lanelet::utils::getArcCoordinates(
@@ -1094,9 +1069,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("lane_change").get_child(getModuleTypeStr()).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);