fix(behavior_path_planner): save original route goal pose (#4876)

* fix(behavior_path_planner): save original route goal pose Signed-off-by: kosuke55 <[email protected]> * fix return Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Sep 5, 2023 · 30ec8f7 · 30ec8f7
1 parent 46a263c
commit 30ec8f7
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 4 deletions.
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
@@ -275,7 +275,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->getGoalPose();
+  const Pose & goal_pose = route_handler->getOriginalGoalPose();
 
   // check if goal_pose is in current_lanes.
   lanelet::ConstLanelet current_lane{};
@@ -511,7 +511,7 @@ void GoalPlannerModule::generateGoalCandidates()
   }
 
   // calculate goal candidates
-  const Pose goal_pose = route_handler->getGoalPose();
+  const Pose goal_pose = route_handler->getOriginalGoalPose();
   refined_goal_pose_ = calcRefinedGoal(goal_pose);
   if (goal_planner_utils::isAllowedGoalModification(route_handler)) {
     goal_searcher_->setPlannerData(planner_data_);
@@ -1548,7 +1548,7 @@ void GoalPlannerModule::printParkingPositionError() const
 bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const
 {
   const auto & route_handler = planner_data_->route_handler;
-  const Pose & goal_pose = route_handler->getGoalPose();
+  const Pose & goal_pose = route_handler->getOriginalGoalPose();
 
   const lanelet::ConstLanelets pull_over_lanes =
     goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_);

diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp
@@ -57,7 +57,7 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith
 
 lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side)
 {
-  const Pose goal_pose = route_handler.getGoalPose();
+  const Pose goal_pose = route_handler.getOriginalGoalPose();
 
   lanelet::ConstLanelet target_shoulder_lane{};
   if (route_handler::RouteHandler::getPullOverTarget(

diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp
@@ -98,6 +98,7 @@ class RouteHandler
   Pose getGoalPose() const;
   Pose getStartPose() const;
   Pose getOriginalStartPose() const;
+  Pose getOriginalGoalPose() const;
   lanelet::Id getGoalLaneId() const;
   bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const;
   std::vector<lanelet::ConstLanelet> getLanesBeforePose(
@@ -378,6 +379,7 @@ class RouteHandler
 
   // save original(not modified) route start pose for start planer execution
   Pose original_start_pose_;
+  Pose original_goal_pose_;
 
   // non-const methods
   void setLaneletsFromRouteMsg();

diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp
@@ -176,6 +176,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg)
     // if get not modified route but new route, reset original start pose
     if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) {
       original_start_pose_ = route_msg.start_pose;
+      original_goal_pose_ = route_msg.goal_pose;
     }
     route_ptr_ = std::make_shared<LaneletRoute>(route_msg);
     is_handler_ready_ = false;
@@ -433,6 +434,10 @@ Pose RouteHandler::getStartPose() const
 
 Pose RouteHandler::getOriginalStartPose() const
 {
+  if (!route_ptr_) {
+    RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet");
+    return Pose();
+  }
   return original_start_pose_;
 }
 
@@ -445,6 +450,15 @@ Pose RouteHandler::getGoalPose() const
   return route_ptr_->goal_pose;
 }
 
+Pose RouteHandler::getOriginalGoalPose() const
+{
+  if (!route_ptr_) {
+    RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet");
+    return Pose();
+  }
+  return original_goal_pose_;
+}
+
 lanelet::Id RouteHandler::getGoalLaneId() const
 {
   if (!route_ptr_ || route_ptr_->segments.empty()) {