Skip to content

Commit

Permalink
fix(start_planner, goal_planner): remove inappropriate reference value (
Browse files Browse the repository at this point in the history
#5618)

Signed-off-by: kosuke55 <[email protected]>
  • Loading branch information
kosuke55 authored Nov 17, 2023
1 parent 9071619 commit 30bc07d
Show file tree
Hide file tree
Showing 3 changed files with 8 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -1751,12 +1751,12 @@ bool GoalPlannerModule::isSafePath() const
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 =
Expand Down Expand Up @@ -1956,7 +1956,7 @@ void GoalPlannerModule::printParkingPositionError() const
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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -742,7 +742,7 @@ void StartPlannerModule::updateStatusAfterBackwardDriving()

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);

Expand Down Expand Up @@ -770,7 +770,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
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);
Expand Down Expand Up @@ -1074,11 +1074,11 @@ bool StartPlannerModule::isSafePath() const
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 =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ lanelet::ConstLanelets getPullOutLanes(
{
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;
Expand Down

0 comments on commit 30bc07d

Please sign in to comment.