From b32681dd3d0dfa70c8ba951ab713e2b3afd25281 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 14 Sep 2022 15:43:20 +0900 Subject: [PATCH] fix(behavior_path_planner): pull over deceleration (#1860) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over_module.cpp | 21 ++++++++----------- 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 43ec94e5240ee..dbf3129897619 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -139,17 +139,17 @@ void PullOverModule::onEntry() *last_received_time_ != planner_data_->route_handler->getRouteHeader().stamp) { // Initialize parallel parking planner status parallel_parking_parameters_ = getGeometricPullOverParameters(); - resetStatus(); } last_received_time_ = std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); // Use refined goal as modified goal when disabling goal research + refined_goal_pose_ = calcRefinedGoal(); if (!parameters_.enable_goal_research) { goal_candidates_.clear(); - GoalCandidate goal_candidate; - goal_candidate.goal_pose = calcRefinedGoal(); + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = refined_goal_pose_; goal_candidate.distance_from_original_goal = 0.0; goal_candidates_.push_back(goal_candidate); } @@ -250,10 +250,9 @@ void PullOverModule::researchGoal() { // Find goals in pull over areas. goal_candidates_.clear(); - const Pose refined_goal_pose = calcRefinedGoal(); for (double dx = -parameters_.backward_goal_search_length; dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { - const Pose search_pose = calcOffsetPose(refined_goal_pose, dx, 0, 0); + const Pose search_pose = calcOffsetPose(refined_goal_pose_, dx, 0, 0); if (checkCollisionWithPose(search_pose)) { continue; } @@ -267,7 +266,7 @@ void PullOverModule::researchGoal() GoalCandidate goal_candidate; goal_candidate.goal_pose = search_pose; goal_candidate.distance_from_original_goal = - std::abs(inverseTransformPose(search_pose, refined_goal_pose).position.x); + std::abs(inverseTransformPose(search_pose, refined_goal_pose_).position.x); goal_candidates_.push_back(goal_candidate); } // Sort with distance from original goal @@ -733,14 +732,13 @@ PathWithLaneId PullOverModule::getReferencePath() const const auto & current_pose = planner_data_->self_pose->pose; const auto & common_parameters = planner_data_->parameters; - const Pose refined_goal_pose = calcRefinedGoal(); if (status_.current_lanes.empty()) { return PathWithLaneId{}; } const auto arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose); + lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); const Pose search_start_pose = calcOffsetPose( - refined_goal_pose, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); // if not approved, stop parking start position or goal search start position. const Pose stop_pose = status_.is_safe ? getParkingStartPose() : search_start_pose; @@ -1023,11 +1021,10 @@ void PullOverModule::setDebugData() // Visualize pull over areas if (parameters_.enable_goal_research) { - const Pose refined_goal_pose = calcRefinedGoal(); const Pose start_pose = - calcOffsetPose(refined_goal_pose, -parameters_.backward_goal_search_length, 0, 0); + calcOffsetPose(refined_goal_pose_, -parameters_.backward_goal_search_length, 0, 0); const Pose end_pose = - calcOffsetPose(refined_goal_pose, parameters_.forward_goal_search_length, 0, 0); + calcOffsetPose(refined_goal_pose_, parameters_.forward_goal_search_length, 0, 0); // marker_array.markers.push_back(createParkingAreaMarker(start_pose, end_pose, 0)); const auto header = planner_data_->route_handler->getRouteHeader(); const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow