fix(lane_change): fix utils' logger

Signed-off-by: Zulfaqar Azmi <[email protected]>
zulfaqar-azmi-t4 · Apr 1, 2024 · c0ea471 · c0ea471
1 parent 28c3c34
commit c0ea471
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 27 deletions.
diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml
@@ -53,6 +53,8 @@ Planning:
   behavior_path_lane_change:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: lane_change.NORMAL
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.lane_change
 
   behavior_velocity_planner:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner

diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -30,6 +30,7 @@
 #include <motion_utils/trajectory/interpolation.hpp>
 #include <motion_utils/trajectory/path_with_lane_id.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
+#include <rclcpp/logger.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
@@ -46,6 +47,18 @@
 #include <optional>
 #include <string>
 #include <vector>
+namespace
+{
+rclcpp::Logger get_logger_for_utils()
+{
+  return rclcpp::get_logger("planning")
+    .get_child("scenario_planning")
+    .get_child("lane_driving")
+    .get_child("behavior_planning")
+    .get_child("behavior_path_planner")
+    .get_child("lane_change");
+}
+}  // namespace
 
 namespace behavior_path_planner::utils::lane_change
 {
@@ -328,17 +341,13 @@ std::optional<LaneChangePath> constructCandidatePath(
   path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration));
 
   if (!path_shifter.generate(&shifted_path, offset_back)) {
-    RCLCPP_DEBUG(
-      rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"),
-      "failed to generate shifted path.");
+    RCLCPP_DEBUG(get_logger_for_utils(), "failed to generate shifted path.");
   }
 
   // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path
   // shifter.
   if (shifted_path.path.points.size() < shift_line.end_idx + 1) {
-    RCLCPP_DEBUG(
-      rclcpp::get_logger("behavior_path_planner").get_child("utils").get_child(__func__),
-      "path points are removed by PathShifter.");
+    RCLCPP_DEBUG(get_logger_for_utils(), "path points are removed by PathShifter.");
     return std::nullopt;
   }
 
@@ -349,19 +358,14 @@ std::optional<LaneChangePath> constructCandidatePath(
   candidate_path.info = lane_change_info;
 
   RCLCPP_DEBUG(
-    rclcpp::get_logger("behavior_path_planner")
-      .get_child("lane_change")
-      .get_child("util")
-      .get_child("constructCandidatePath"),
-    "prepare_length: %f, lane_change: %f", prepare_length, lane_changing_length);
+    get_logger_for_utils(), "prepare_length: %f, lane_change: %f", prepare_length,
+    lane_changing_length);
 
   const auto lane_change_end_idx =
     motion_utils::findNearestIndex(shifted_path.path.points, candidate_path.info.lane_changing_end);
 
   if (!lane_change_end_idx) {
-    RCLCPP_ERROR_STREAM(
-      rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"),
-      "lane change end idx not found on target path.");
+    RCLCPP_ERROR(get_logger_for_utils(), "lane change end idx not found on target path.");
     return std::nullopt;
   }
 
@@ -413,7 +417,7 @@ PathWithLaneId getReferencePathFromTargetLane(
   const double lane_changing_length, const double forward_path_length,
   const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer)
 {
-  const ArcCoordinates lane_change_start_arc_position =
+  const auto lane_change_start_arc_position =
     lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose);
 
   const double s_start = lane_change_start_arc_position.length;
@@ -428,15 +432,14 @@ PathWithLaneId getReferencePathFromTargetLane(
     return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer);
   });
 
-  RCLCPP_DEBUG(
-    rclcpp::get_logger("behavior_path_planner")
-      .get_child("lane_change")
-      .get_child("util")
-      .get_child("getReferencePathFromTargetLane"),
-    "start: %f, end: %f", s_start, s_end);
+  RCLCPP_DEBUG(get_logger_for_utils(), "start: %f, end: %f", s_start, s_end);
 
   constexpr double epsilon = 1e-4;
   if (s_end - s_start + epsilon < lane_changing_length) {
+    RCLCPP_DEBUG(
+      get_logger_for_utils(),
+      "Reject: Distance %.4f [m] is less than lane changing length %.4f [m].",
+      (s_end - s_start + epsilon), lane_changing_length);
     return PathWithLaneId();
   }
 
@@ -471,12 +474,7 @@ ShiftLine getLaneChangingShiftLine(
   shift_line.end_idx =
     motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position);
 
-  RCLCPP_DEBUG(
-    rclcpp::get_logger("behavior_path_planner")
-      .get_child("lane_change")
-      .get_child("util")
-      .get_child("getLaneChangingShiftLine"),
-    "shift_line distance: %f", shift_length);
+  RCLCPP_DEBUG(get_logger_for_utils(), "shift_line distance: %f", shift_length);
   return shift_line;
 }