From c03ca7865e2af67694431a23ca4e17ae1a7b53c4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:16:59 +0900 Subject: [PATCH] refactor(start/goal_planner): minor refactor like const and reference (#4897) Signed-off-by: kosuke55 --- .../utils/start_planner/freespace_pull_out.hpp | 2 +- .../utils/start_planner/geometric_pull_out.hpp | 2 +- .../start_planner/pull_out_planner_base.hpp | 2 +- .../utils/start_planner/shift_pull_out.hpp | 2 +- .../goal_planner/goal_planner_module.cpp | 16 ++++++++-------- .../start_planner/start_planner_module.cpp | 14 +++++++------- .../geometric_parallel_parking.cpp | 1 - .../utils/start_planner/freespace_pull_out.cpp | 4 ++-- .../utils/start_planner/geometric_pull_out.cpp | 2 +- .../src/utils/start_planner/shift_pull_out.cpp | 2 +- 10 files changed, 23 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/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 plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/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 plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/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 plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/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 & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b9fd3ef781d1a..8403c9904c624 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/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::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 = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 949b6ae633b64..e652dde998b39 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/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 = diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 27dbfb532c1fc..99d109dd0bece 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -447,7 +447,6 @@ std::vector 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); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional 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 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); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 29abdb08c5c55..f4f65474d4a30 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/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 GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 7352821e140be..f76aca793d571 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -40,7 +40,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional 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;