From d1ce2fd64ec254bd7f87da359e0ad510a09af889 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Thu, 28 Nov 2024 18:04:15 +0900 Subject: [PATCH] make current lanes as member variables to reduce computation time and reduce code repetition Signed-off-by: Daniel Sanchez --- .../start_planner_module.hpp | 2 + .../src/start_planner_module.cpp | 46 +++++++++---------- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index bb70ae2638056..70537d57f5d80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -163,6 +163,8 @@ class StartPlannerModule : public SceneModuleInterface PlannerData planner_data; ModuleStatus current_status; PullOutStatus main_thread_pull_out_status; + lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets extended_current_lanelets; bool is_stopped; StartPlannerData clone() const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 210afdcd7c1ea..02ab01f9f0b3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -21,6 +21,7 @@ #include "autoware/behavior_path_start_planner_module/util.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include #include #include #include @@ -380,7 +381,7 @@ bool StartPlannerModule::isModuleRunning() const bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets & current_lanes = start_planner_data_->current_lanelets; const double lateral_distance_to_center_lane = lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; @@ -426,7 +427,8 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & const auto & route_handler = planner_data_->route_handler; const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - const auto target_lanes = utils::getCurrentLanes(planner_data_); + const auto & target_lanes = start_planner_data_->current_lanelets; + if (target_lanes.empty()) return false; // Define functions to get distance between a point and a lane's boundaries. @@ -821,12 +823,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() waitApproval(); - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - auto stop_path = status_.driving_forward ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -1323,11 +1319,7 @@ bool StartPlannerModule::hasReachedPullOutEnd() const const auto current_pose = planner_data_->self_odometry->pose.pose; // check that ego has passed pull out end point - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const auto & current_lanes = start_planner_data_->extended_current_lanelets; const auto arclength_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose); const auto arclength_pull_out_end = @@ -1372,7 +1364,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() path.points, status_.pull_out_path.start_pose.position); const auto shift_end_idx = autoware::motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes(planner_data_); + const auto & current_lanes = start_planner_data_->extended_current_lanelets; const auto is_ignore_signal = [this](const lanelet::Id & id) { if (!ignore_signal_.has_value()) { @@ -1450,12 +1442,7 @@ bool StartPlannerModule::isSafePath() const planner_data_->self_odometry->twist.twist.linear.y); const auto & dynamic_object = planner_data_->dynamic_object; const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const auto & current_lanes = start_planner_data_->extended_current_lanelets; // for ego predicted path const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); @@ -1575,11 +1562,7 @@ std::optional StartPlannerModule::planFreespacePath( const double end_pose_search_end_distance = parameters.end_pose_search_end_distance; const double end_pose_search_interval = parameters.end_pose_search_interval; - const double backward_path_length = - planner_data->parameters.backward_path_length + parameters.max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const auto & current_lanes = start_planner_data_->extended_current_lanelets; const auto current_arc_coords = lanelet::utils::getArcCoordinates(current_lanes, current_pose); @@ -1946,5 +1929,18 @@ void StartPlannerModule::StartPlannerData::update( current_status = current_status_; main_thread_pull_out_status = pull_out_status_; is_stopped = is_stopped_; + + // compute current lanes + { + auto planner_data_ptr = std::make_shared(planner_data); + + current_lanelets = utils::getCurrentLanes(planner_data_ptr); + + const double backward_path_length = + planner_data_.parameters.backward_path_length + parameters_.max_back_distance; + extended_current_lanelets = utils::getExtendedCurrentLanes( + planner_data_ptr, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + } } } // namespace autoware::behavior_path_planner