refactor(start/goal_planner): minor refactor like const and reference (…

…#4897)

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp

@@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase
 
   PlannerType getPlannerType() override { return PlannerType::FREESPACE; }
 
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose end_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & end_pose) override;
 
 protected:
   std::unique_ptr<AbstractPlanningAlgorithm> planner_;

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp

@@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase
   explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters);
 
   PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; };
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   GeometricParallelParking planner_;
   ParallelParkingParameters parallel_parking_parameters_;

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp

@@ -60,7 +60,7 @@ class PullOutPlannerBase
   }
 
   virtual PlannerType getPlannerType() = 0;
-  virtual boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) = 0;
+  virtual boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) = 0;
 
 protected:
   std::shared_ptr<const PlannerData> planner_data_;

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp

@@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase
     std::shared_ptr<LaneDepartureChecker> & lane_departure_checker);
 
   PlannerType getPlannerType() override { return PlannerType::SHIFT; };
-  boost::optional<PullOutPath> plan(Pose start_pose, Pose goal_pose) override;
+  boost::optional<PullOutPath> plan(const Pose & start_pose, const Pose & goal_pose) override;
 
   std::vector<PullOutPath> calcPullOutPaths(
     const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes,

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

@@ -276,7 +276,7 @@ bool GoalPlannerModule::isExecutionRequested() const
 
   const auto & route_handler = planner_data_->route_handler;
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
-  const Pose & goal_pose = route_handler->getOriginalGoalPose();
+  const Pose goal_pose = route_handler->getOriginalGoalPose();
 
   // check if goal_pose is in current_lanes.
   lanelet::ConstLanelet current_lane{};
@@ -1145,7 +1145,7 @@ bool GoalPlannerModule::isStuck()
 
 bool GoalPlannerModule::hasFinishedCurrentPath()
 {
-  const auto & current_path_end = getCurrentPath().points.back();
+  const auto current_path_end = getCurrentPath().points.back();
   const auto & self_pose = planner_data_->self_odometry->pose.pose;
   const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) <
                               parameters_->th_arrived_distance;
@@ -1469,9 +1469,9 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData(
 
 bool GoalPlannerModule::isSafePath() const
 {
-  const auto & pull_over_path = getCurrentPath();
+  const auto pull_over_path = getCurrentPath();
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
-  const auto & current_velocity = std::hypot(
+  const double current_velocity = std::hypot(
     planner_data_->self_odometry->twist.twist.linear.x,
     planner_data_->self_odometry->twist.twist.linear.y);
   const auto & dynamic_object = planner_data_->dynamic_object;
@@ -1480,7 +1480,7 @@ bool GoalPlannerModule::isSafePath() const
   const auto current_lanes = utils::getExtendedCurrentLanes(
     planner_data_, backward_path_length, std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
-  const auto & pull_over_lanes =
+  const auto pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);
   const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points);
   const auto & common_param = planner_data_->parameters;
@@ -1493,16 +1493,16 @@ bool GoalPlannerModule::isSafePath() const
   RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx);
   utils::start_goal_planner_common::updatePathProperty(
     ego_predicted_path_params_, terminal_velocity_and_accel);
-  const auto & ego_predicted_path =
+  const auto ego_predicted_path =
     behavior_path_planner::utils::path_safety_checker::createPredictedPath(
       ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity,
       ego_seg_idx);
 
-  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_);
 
-  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 =

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

@@ -110,7 +110,7 @@ bool StartPlannerModule::isExecutionRequested() const
 {
   // TODO(Sugahara): if required lateral shift distance is small, don't engage this module.
   // Execute when current pose is near route start pose
-  const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose();
+  const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
   if (
     tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) >
@@ -119,7 +119,7 @@ bool StartPlannerModule::isExecutionRequested() const
   }
 
   // Check if ego arrives at goal
-  const Pose & goal_pose = planner_data_->route_handler->getGoalPose();
+  const Pose goal_pose = planner_data_->route_handler->getGoalPose();
   if (
     tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) <
     parameters_->th_arrived_distance) {
@@ -953,9 +953,9 @@ bool StartPlannerModule::isSafePath() const
 {
   // TODO(Sugahara): should safety check for backward path
 
-  const auto & pull_out_path = getCurrentPath();
+  const auto pull_out_path = getCurrentPath();
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
-  const auto & current_velocity = std::hypot(
+  const double current_velocity = std::hypot(
     planner_data_->self_odometry->twist.twist.linear.x,
     planner_data_->self_odometry->twist.twist.linear.y);
   const auto & dynamic_object = planner_data_->dynamic_object;
@@ -972,15 +972,15 @@ bool StartPlannerModule::isSafePath() const
       status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx);
   utils::start_goal_planner_common::updatePathProperty(
     ego_predicted_path_params_, terminal_velocity_and_accel);
-  const auto & ego_predicted_path =
+  const auto ego_predicted_path =
     behavior_path_planner::utils::path_safety_checker::createPredictedPath(
       ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity,
       ego_seg_idx);
 
-  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_);
 
-  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/geometric_parallel_parking/geometric_parallel_parking.cpp

@@ -447,7 +447,6 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   if (std::abs(end_pose_offset) > 0) {
     PathPointWithLaneId straight_point{};
     straight_point.point.pose = goal_pose;
-    // setLaneIds(straight_point);
     path_turn_right.points.push_back(straight_point);
   }
 

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

@@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut(
   }
 }
 
-boost::optional<PullOutPath> FreespacePullOut::plan(const Pose start_pose, const Pose end_pose)
+boost::optional<PullOutPath> FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose)
 {
   const auto & route_handler = planner_data_->route_handler;
   const double backward_path_length = planner_data_->parameters.backward_path_length;
@@ -86,7 +86,7 @@ boost::optional<PullOutPath> FreespacePullOut::plan(const Pose start_pose, const
   }
 
   // push back generate road lane path between end pose and goal pose to last path
-  const auto & goal_pose = route_handler->getGoalPose();
+  const Pose goal_pose = route_handler->getGoalPose();
   constexpr double offset_from_end_pose = 1.0;
   const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose);
   const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0);

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

@@ -36,7 +36,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame
   planner_.setParameters(parallel_parking_parameters_);
 }
 
-boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_pose)
+boost::optional<PullOutPath> GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose)
 {
   PullOutPath output;
 

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

@@ -40,7 +40,7 @@ ShiftPullOut::ShiftPullOut(
 {
 }
 
-boost::optional<PullOutPath> ShiftPullOut::plan(Pose start_pose, Pose goal_pose)
+boost::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose)
 {
   const auto & route_handler = planner_data_->route_handler;
   const auto & common_parameters = planner_data_->parameters;