fix(behavior_path_planner): pull over deceleration (autowarefoundatio…

…n#1860) Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]>
tier4 · Nov 30, 2022 · b32681d · b32681d
1 parent 43f2542
commit b32681d
Showing 1 changed file with 9 additions and 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
@@ -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<rclcpp::Time>(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