fix(start_planner, goal_planner): remove inappropriate reference value

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -264,7 +264,7 @@
   if (thread_safe_data_.get_goal_candidates().empty()) {
     goal_searcher_->setPlannerData(planner_data_);
     goal_searcher_->setReferenceGoal(
       calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose()));
     thread_safe_data_.set_goal_candidates(generateGoalCandidates());
   }
 
@@ -312,7 +312,7 @@
   resetPathCandidate();
   resetPathReference();
   debug_marker_.markers.clear();
   prev_data_.reset();
   last_approval_data_.reset();
 }
 
@@ -522,7 +522,7 @@
   return false;
 }
 
 bool GoalPlannerModule::canReturnToLaneParking()
 {
   // return only before starting free space parking
   if (!isStopped()) {
@@ -551,25 +551,25 @@
   const bool is_close_to_path =
     std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance;
   if (!is_close_to_path) {
     return false;
   }
 
   return true;
 }
 
 GoalCandidates GoalPlannerModule::generateGoalCandidates() const
 {
   // calculate goal candidates
   const auto & route_handler = planner_data_->route_handler;
   if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
     return goal_searcher_->search();
   }
 
   // NOTE:
   // currently since pull over is performed only when isAllowedGoalModification is true,
   // never be in the following process.
   GoalCandidate goal_candidate{};
   goal_candidate.goal_pose = route_handler->getOriginalGoalPose();
   goal_candidate.distance_from_original_goal = 0.0;
   GoalCandidates goal_candidates{};
   goal_candidates.push_back(goal_candidate);
@@ -608,7 +608,7 @@
 
   if (parameters_->use_object_recognition) {
     // Create a map of PullOverPath pointer to largest collision check margin
     auto calcLargestMargin = [&](const PullOverPath & pull_over_path) {
       // check has safe goal
       const auto goal_candidate_it = std::find_if(
         goal_candidates.begin(), goal_candidates.end(),
@@ -619,17 +619,17 @@
         return 0.0;
       }
       // calc largest margin
       std::vector<double> scale_factors{3.0, 2.0, 1.0};
       const double margin = parameters_->object_recognition_collision_check_margin;
       const auto resampled_path =
         utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
       for (const auto & scale_factor : scale_factors) {
         if (!checkObjectsCollision(resampled_path, margin * scale_factor)) {
           return margin * scale_factor;
         }
       }
       return 0.0;
     };
 
     // Create a map of PullOverPath pointer to largest collision check margin
     std::map<size_t, double> path_id_to_margin_map;
@@ -638,13 +638,13 @@
     }
 
     // sorts in descending order so the item with larger margin comes first
     std::stable_sort(
       sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(),
       [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) {
         if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) {
           return false;
         }
         return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id];
       });
   }
 
@@ -663,7 +663,7 @@
   return sorted_pull_over_path_candidates;
 }
 
 std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates, const double collision_check_margin) const
 {
@@ -683,19 +683,19 @@
     }
 
     const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5);
     if (
       parameters_->use_object_recognition &&
       checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) {
       continue;
     }
 
     if (
       parameters_->use_occupancy_grid_for_path_collision_check &&
       checkOccupancyGridCollision(resampled_path)) {
       continue;
     }
 
     return std::make_pair(pull_over_path, *goal_candidate_it);
   }
 
   return {};
@@ -937,11 +937,11 @@
     // select safe path from lane parking pull over path candidates
     // and set it to thread_safe_data_
 
     thread_safe_data_.clearPullOverPath();
 
     // update goal candidates
     goal_searcher_->setPlannerData(planner_data_);
     auto goal_candidates = thread_safe_data_.get_goal_candidates();
     goal_searcher_->update(goal_candidates);
 
     // update pull over path candidates
@@ -1016,7 +1016,7 @@
   setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath());
 }
 
 void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output)
 {
   if (prev_data_.found_path || !prev_data_.stop_path) {
     prev_data_.stop_path = output.path;
@@ -1098,9 +1098,9 @@
   const double pull_over_velocity = parameters_->pull_over_velocity;
 
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
 
   if (current_lanes.empty()) {
     return PathWithLaneId{};
@@ -1189,9 +1189,9 @@
 
   // generate stop reference path
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
 
   const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length;
   const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length);
@@ -1268,7 +1268,7 @@
     return false;
   }
 
   if (
     parameters_->use_object_recognition &&
     checkObjectsCollision(
       thread_safe_data_.get_pull_over_path()->getCurrentPath(),
@@ -1276,10 +1276,10 @@
     return true;
   }
 
   if (
     parameters_->use_occupancy_grid_for_path_collision_check &&
     checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) {
     return true;
   }
 
   return false;
@@ -1374,16 +1374,16 @@
   return turn_signal;
 }
 
 bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const
 {
   if (!occupancy_grid_map_) {
     return false;
   }
   const bool check_out_of_range = false;
   return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range);
 }
 
 bool GoalPlannerModule::checkObjectsCollision(
   const PathWithLaneId & path, const double collision_check_margin,
   const bool update_debug_data) const
 {
@@ -1442,8 +1442,8 @@
       tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0);
     const auto ego_polygon = tier4_autoware_utils::toFootprint(
       lateral_offset_pose,
       planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin,
       planner_data_->parameters.base_link2rear + collision_check_margin,
       planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 +
         std::abs(extra_lateral_margin));
     ego_polygons_expanded.push_back(ego_polygon);
@@ -1524,7 +1524,7 @@
     path.points, current_pose.position, ego_idx, pose.position, target_idx);
 }
 
 void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
 {
   // decelerate before the search area start
   const auto closest_goal_candidate =
@@ -1534,7 +1534,7 @@
     -approximate_pull_over_distance_);
   auto & first_path = pull_over_path.partial_paths.front();
   if (decel_pose) {
     decelerateBeforeSearchStart(*decel_pose, first_path);
     return;
   }
 
@@ -1543,11 +1543,11 @@
     planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk,
     parameters_->pull_over_velocity);
   for (auto & p : first_path.points) {
     const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose);
     if (min_decel_distance && distance_from_ego < *min_decel_distance) {
       continue;
     }
     p.point.longitudinal_velocity_mps = std::min(
       p.point.longitudinal_velocity_mps, static_cast<float>(parameters_->pull_over_velocity));
   }
 }
@@ -1726,7 +1726,7 @@
   const auto & dynamic_object = planner_data_->dynamic_object;
   const auto & route_handler = planner_data_->route_handler;
   const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, parameters_->backward_goal_search_length,
     parameters_->forward_goal_search_length,
     /*forward_only_in_route*/ false);
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
@@ -1751,12 +1751,12 @@
       ego_seg_idx, is_object_front, limit_to_max_velocity);
 
   // filtering objects with velocity, position and class
-  const auto & filtered_objects = utils::path_safety_checker::filterObjects(
+  const auto filtered_objects = utils::path_safety_checker::filterObjects(
     dynamic_object, route_handler, pull_over_lanes, current_pose.position,
     objects_filtering_params_);
 
   // filtering objects based on the current position's lane
-  const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
+  const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
     pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);
 
   const double hysteresis_factor =
@@ -1856,7 +1856,7 @@
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST,
       tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0),
       tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999));
     const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
     for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) {
       for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) {
         const auto & current_point = ego_polygon.outer().at(ep_idx);
@@ -1956,7 +1956,7 @@
 bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const
 {
   const auto & route_handler = planner_data_->route_handler;
-  const Pose & goal_pose = route_handler->getOriginalGoalPose();
+  const Pose goal_pose = route_handler->getOriginalGoalPose();
 
   const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *(route_handler), left_side_parking_, parameters_->backward_goal_search_length,

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -228,12 +228,12 @@
   return true;
 }
 
 bool StartPlannerModule::canTransitSuccessState()
 {
   return hasFinishedPullOut();
 }
 
 bool StartPlannerModule::canTransitIdleToRunningState()
 {
   return isActivated() && !isWaitingApproval();
 }
@@ -742,7 +742,7 @@
 
 PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const
 {
-  const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose();
+  const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
 
@@ -770,7 +770,7 @@
   const PathWithLaneId & back_path_from_start_pose) const
 {
   std::vector<Pose> pull_out_start_pose_candidates{};
-  const auto & start_pose = planner_data_->route_handler->getOriginalStartPose();
+  const auto start_pose = planner_data_->route_handler->getOriginalStartPose();
   const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_);
   const auto pull_out_lanes = start_planner_utils::getPullOutLanes(
     planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance);
@@ -1074,11 +1074,11 @@
       is_object_front, limit_to_max_velocity);
 
   // filtering objects with velocity, position and class
-  const auto & filtered_objects = utils::path_safety_checker::filterObjects(
+  const auto filtered_objects = utils::path_safety_checker::filterObjects(
     dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_);
 
   // filtering objects based on the current position's lane
-  const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
+  const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
     current_lanes, route_handler, filtered_objects, objects_filtering_params_);
 
   const double hysteresis_factor =

planning/behavior_path_planner/src/utils/start_planner/util.cpp

@@ -89,7 +89,7 @@
 {
   const double & vehicle_width = planner_data->parameters.vehicle_width;
   const auto & route_handler = planner_data->route_handler;
-  const auto & start_pose = planner_data->route_handler->getOriginalStartPose();
+  const auto start_pose = planner_data->route_handler->getOriginalStartPose();
 
   lanelet::ConstLanelet current_shoulder_lane;
   lanelet::ConstLanelets shoulder_lanes;