refactor(lane_change): add debug log

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -14,6 +14,10 @@ behavior_path_planner_avoidance:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
 
+behavior_path_planner_lane_change:
+  - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+    logger_name: lane_change
+
 behavior_velocity_planner:
   - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner
     logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -271,6 +271,9 @@ class LaneChangeBase
   mutable LaneChangeTargetObjects debug_filtered_objects_{};
   mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
+
+  rclcpp::Logger logger_ = rclcpp::get_logger("lane_change");
+  mutable rclcpp::Clock clock_{RCL_ROS_TIME};
 };
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -173,7 +173,6 @@ class NormalLaneChange : public LaneChangeBase
 
   double getStopTime() const { return stop_time_; }
 
-  rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
   double stop_time_{0.0};
 };
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -642,6 +642,8 @@
 
   // if max acc is not positive, then we do the normal sampling
   if (max_acc <= 0.0) {
+    RCLCPP_DEBUG(
+      logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc);
     return utils::lane_change::getAccelerationValues(
       min_acc, max_acc, longitudinal_acc_sampling_num);
   }
@@ -650,6 +652,9 @@
   const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc);
 
   if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) {
+    RCLCPP_DEBUG(
+      logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc,
+      max_acc);
     return utils::lane_change::getAccelerationValues(
       min_acc, max_acc, longitudinal_acc_sampling_num);
   }
@@ -658,7 +663,8 @@
   if (isVehicleStuckByObstacle(current_lanes)) {
     auto clock = rclcpp::Clock(RCL_ROS_TIME);
     RCLCPP_INFO_THROTTLE(
-      logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration.");
+      logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc,
+      max_acc);
     return utils::lane_change::getAccelerationValues(
       min_acc, max_acc, longitudinal_acc_sampling_num);
   }
@@ -668,12 +674,17 @@
   if (route_handler.isInGoalRouteSection(target_lanes.back())) {
     const auto goal_pose = route_handler.getGoalPose();
     if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) {
+      RCLCPP_DEBUG(
+        logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc);
       return {max_acc};
     }
   } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) {
+    RCLCPP_DEBUG(
+      logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc);
     return {max_acc};
   }
 
+  RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc);
   return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num);
 }
 
@@ -1021,255 +1032,285 @@
 {
   object_debug_.clear();
   if (current_lanes.empty() || target_lanes.empty()) {
+    RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
     return false;
   }
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
 
   const auto backward_path_length = common_parameters.backward_path_length;
   const auto forward_path_length = common_parameters.forward_path_length;
   const auto minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity;
   const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num;
 
   // get velocity
   const auto current_velocity = getEgoVelocity();
 
   // get sampling acceleration values
   const auto longitudinal_acc_sampling_values =
     sampleLongitudinalAccValues(current_lanes, target_lanes);
 
   const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back());
 
   const double lane_change_buffer = utils::calcMinimumLaneChangeLength(
     common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()));
   const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength(
     common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()));
 
   const auto dist_to_end_of_current_lanes =
     utils::getDistanceToEndOfLane(getEgoPose(), current_lanes);
 
   const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes);
 
   const auto sorted_lane_ids =
     utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes);
 
   const auto target_neighbor_preferred_lane_poly_2d =
     utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_);
   if (target_neighbor_preferred_lane_poly_2d.empty()) {
+    RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
     return false;
   }
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
   debug_filtered_objects_ = target_objects;
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
   candidate_paths->reserve(
     longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size());
 
+  RCLCPP_DEBUG(
+    logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu",
+    prepare_durations.size(), longitudinal_acc_sampling_values.size());
+
   for (const auto & prepare_duration : prepare_durations) {
     for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) {
+      const auto debug_print = [&](const auto & s) {
+        RCLCPP_DEBUG_STREAM(
+          logger_, "  -  " << s << " : prep_time = " << prepare_duration
+                           << ", lon_acc = " << sampled_longitudinal_acc);
+      };
+
       // get path on original lanes
       const auto prepare_velocity = std::max(
         current_velocity + sampled_longitudinal_acc * prepare_duration,
         minimum_lane_changing_velocity);
 
       // compute actual longitudinal acceleration
       const double longitudinal_acc_on_prepare =
         (prepare_duration < 1e-3) ? 0.0
                                   : ((prepare_velocity - current_velocity) / prepare_duration);
 
       const double prepare_length =
         current_velocity * prepare_duration +
         0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2);
 
       auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length);
 
       if (prepare_segment.points.empty()) {
-        RCLCPP_DEBUG(logger_, "prepare segment is empty!!");
+        debug_print("prepare segment is empty...? Unexpected.");
         continue;
       }
 
       // lane changing start getEgoPose() is at the end of prepare segment
       const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose;
       const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet(
         current_lanes, target_lanes.front(), lane_changing_start_pose);
 
       // Check if the lane changing start point is not on the lanes next to target lanes,
       if (target_length_from_lane_change_start_pose > 0.0) {
-        RCLCPP_DEBUG(
-          logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!");
+        debug_print("lane change start getEgoPose() is behind target lanelet!");
         break;
       }
 
       const auto shift_length =
         lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose);
 
       const auto initial_lane_changing_velocity = prepare_velocity;
       const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps;
 
       // get lateral acceleration range
       const auto [min_lateral_acc, max_lateral_acc] =
         common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity);
       const auto lateral_acc_resolution =
         std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num;
-      constexpr double lateral_acc_epsilon = 0.01;
 
-      for (double lateral_acc = min_lateral_acc;
-           lateral_acc < max_lateral_acc + lateral_acc_epsilon;
-           lateral_acc += lateral_acc_resolution) {
+      std::vector<double> sample_lat_acc;
+      constexpr double eps = 0.01;
+      for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) {
+        sample_lat_acc.push_back(a);
+      }
+      RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
+
+      for (const auto & lateral_acc : sample_lat_acc) {
+        const auto debug_print = [&](const auto & s) {
+          RCLCPP_DEBUG_STREAM(
+            logger_, "    -  " << s << " : prep_time = " << prepare_duration << ", lon_acc = "
+                               << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc);
+        };
+
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc);
         const double longitudinal_acc_on_lane_changing =
           utils::lane_change::calcLaneChangingAcceleration(
             initial_lane_changing_velocity, max_path_velocity, lane_changing_time,
             sampled_longitudinal_acc);
         const auto lane_changing_length =
           initial_lane_changing_velocity * lane_changing_time +
           0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time;
         const auto terminal_lane_changing_velocity =
           initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time;
         utils::lane_change::setPrepareVelocity(
           prepare_segment, current_velocity, terminal_lane_changing_velocity);
 
         if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) {
-          RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!");
+          debug_print("Reject: length of lane changing path is longer than length to goal!!");
           continue;
         }
 
         if (is_goal_in_route) {
           const double s_start =
             lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length;
           const double s_goal =
             lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
           const auto num =
             std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction));
           const double backward_buffer =
             num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane;
           const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer;
           if (
             s_start + lane_changing_length + finish_judge_buffer + backward_buffer +
               next_lane_change_buffer >
             s_goal) {
-            RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!");
+            debug_print("Reject: length of lane changing path is longer than length to goal!!");
             continue;
           }
         }
 
         const auto target_segment = getTargetSegment(
           target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length,
           initial_lane_changing_velocity, next_lane_change_buffer);
 
         if (target_segment.points.empty()) {
-          RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong...");
+          debug_print("Reject: target segment is empty!! something wrong...");
           continue;
         }
 
         const lanelet::BasicPoint2d lc_start_point(
           prepare_segment.points.back().point.pose.position.x,
           prepare_segment.points.back().point.pose.position.y);
 
         const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength(
           target_lanes, 0, std::numeric_limits<double>::max());
         const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon();
 
         const auto is_valid_start_point =
           boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
           boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
 
         if (!is_valid_start_point) {
-          // lane changing points are not inside of the target preferred lanes or its neighbors
+          debug_print(
+            "Reject: lane changing points are not inside of the target preferred lanes or its "
+            "neighbors");
           continue;
         }
 
         const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval(
           lane_changing_length, initial_lane_changing_velocity);
         const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane(
           route_handler, target_lanes, lane_changing_start_pose, target_lane_length,
           lane_changing_length, forward_path_length, resample_interval, is_goal_in_route,
           next_lane_change_buffer);
 
         if (target_lane_reference_path.points.empty()) {
-          RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!");
+          debug_print("Reject: target_lane_reference_path is empty!!");
           continue;
         }
 
         LaneChangeInfo lane_change_info;
         lane_change_info.longitudinal_acceleration =
           LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
         lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
         lane_change_info.velocity =
           LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
         lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
         lane_change_info.current_lanes = current_lanes;
         lane_change_info.target_lanes = target_lanes;
         lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
         lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
         lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
           prepare_segment, target_segment, target_lane_reference_path, shift_length);
         lane_change_info.lateral_acceleration = lateral_acc;
         lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
 
         const auto candidate_path = utils::lane_change::constructCandidatePath(
           lane_change_info, prepare_segment, target_segment, target_lane_reference_path,
           sorted_lane_ids);
 
         if (!candidate_path) {
-          RCLCPP_DEBUG(logger_, "no candidate path!!");
+          debug_print("Reject: failed to generate candidate path!!");
           continue;
         }
 
         if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) {
-          RCLCPP_DEBUG(logger_, "invalid candidate path!!");
+          debug_print("Reject: invalid candidate path!!");
           continue;
         }
 
         if (
           lane_change_parameters_->regulate_on_crosswalk &&
           !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) {
-          RCLCPP_DEBUG(logger_, "Including crosswalk!!");
           if (getStopTime() < lane_change_parameters_->stop_time_threshold) {
+            debug_print("Reject: including crosswalk!!");
             continue;
           }
-          auto clock{rclcpp::Clock{RCL_ROS_TIME}};
-          RCLCPP_WARN_THROTTLE(
-            logger_, clock, 1000, "Stop time is over threshold. Allow lane change in crosswalk.");
+          RCLCPP_INFO_THROTTLE(
+            logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk.");
         }
 
         if (
           lane_change_parameters_->regulate_on_intersection &&
           !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) {
-          RCLCPP_DEBUG(logger_, "Including intersection!!");
           if (getStopTime() < lane_change_parameters_->stop_time_threshold) {
+            debug_print("Reject: including intersection!!");
             continue;
           }
           RCLCPP_WARN_STREAM(
             logger_, "Stop time is over threshold. Allow lane change in intersection.");
         }
 
         candidate_paths->push_back(*candidate_path);
         if (
           !is_stuck &&
           utils::lane_change::passParkedObject(
             route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
             is_goal_in_route, *lane_change_parameters_, object_debug_)) {
+          debug_print(
+            "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
+            "lane change.");
           return false;
         }
 
         if (!check_safety) {
+          debug_print("ACCEPT!!!: it is valid (and safety check is skipped).");
           return false;
         }
 
         const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
           *candidate_path, target_objects, rss_params, is_stuck, object_debug_);
 
         if (is_safe) {
+          debug_print("ACCEPT!!!: it is valid and safe!");
           return true;
         }
+
+        debug_print("Reject: sampled path is not safe.");
       }
     }
   }
 
+  RCLCPP_DEBUG(logger_, "No safety path found.");
   return false;
 }
 
@@ -1630,11 +1671,13 @@
 {
   // Ego is still moving, not in stuck
   if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) {
+    RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck");
     return false;
   }
 
   // Ego is just stopped, not sure it is in stuck yet.
   if (getStopTime() < lane_change_parameters_->stop_time_threshold) {
+    RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime());
     return false;
   }
 
@@ -1652,11 +1695,13 @@
 
     const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance;
     if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) {
+      RCLCPP_DEBUG(logger_, "Stationary object is in front of ego.");
       return true;  // Stationary object is in front of ego.
     }
   }
 
   // No stationary objects found in obstacle_check_distance
+  RCLCPP_DEBUG(logger_, "No stationary objects found in obstacle_check_distance");
   return false;
 }