refactor(goal_planner): remove modified_goal in ThreadDafeData

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -406,7 +406,7 @@ class GoalPlannerModule : public SceneModuleInterface
   BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
   BehaviorModuleOutput planPullOverAsCandidate(const PullOverContextData & context_data);
-  std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
+  std::optional<PullOverPath> selectPullOverPath(
     const PullOverContextData & context_data,
     const std::vector<PullOverPath> & pull_over_path_candidates,
     const GoalCandidates & goal_candidates) const;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp

@@ -37,8 +37,9 @@ class FreespacePullOver : public PullOverPlannerBase
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; }
 
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   const double velocity_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp

@@ -43,8 +43,9 @@ class GeometricPullOver : public PullOverPlannerBase
   Pose getCl() const { return planner_.getCl(); }
 
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
   std::vector<PullOverPath> generatePullOverPaths(
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
+#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
 #include "autoware/behavior_path_planner_common/data_manager.hpp"
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -41,19 +42,21 @@ struct PullOverPath
 {
 public:
   static std::optional<PullOverPath> create(
-    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
+    const PullOverPlannerType & type, const size_t id,
     const std::vector<PathWithLaneId> & partial_paths, const Pose & start_pose,
-    const Pose & end_pose,
+    const GoalCandidate & modified_goal_pose,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPath(const PullOverPath & other);
   PullOverPath & operator=(const PullOverPath & other) = default;
 
   PullOverPlannerType type() const { return type_; }
-  size_t goal_id() const { return goal_id_; }
+  size_t goal_id() const { return modified_goal_pose_.id; }
   size_t id() const { return id_; }
   Pose start_pose() const { return start_pose_; }
-  Pose end_pose() const { return end_pose_; }
+  Pose end_pose() const { return modified_goal_pose_.goal_pose; }
+  Pose modified_goal_pose() const { return modified_goal_pose_.goal_pose; }
+  GoalCandidate modified_goal() const { return modified_goal_pose_; }
 
   std::vector<PathWithLaneId> & partial_paths() { return partial_paths_; }
   const std::vector<PathWithLaneId> & partial_paths() const { return partial_paths_; }
@@ -86,19 +89,18 @@ struct PullOverPath
 
 private:
   PullOverPath(
-    const PullOverPlannerType & type, const size_t goal_id, const size_t id,
-    const Pose & start_pose, const Pose & end_pose,
-    const std::vector<PathWithLaneId> & partial_paths, const PathWithLaneId & full_path,
-    const PathWithLaneId & parking_path, const std::vector<double> & full_path_curvatures,
+    const PullOverPlannerType & type, const size_t id, const Pose & start_pose,
+    const GoalCandidate & modified_goal_pose, const std::vector<PathWithLaneId> & partial_paths,
+    const PathWithLaneId & full_path, const PathWithLaneId & parking_path,
+    const std::vector<double> & full_path_curvatures,
     const std::vector<double> & parking_path_curvatures, const double full_path_max_curvature,
     const double parking_path_max_curvature,
     const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel);
 
   PullOverPlannerType type_;
-  size_t goal_id_;
+  GoalCandidate modified_goal_pose_;
   size_t id_;
   Pose start_pose_;
-  Pose end_pose_;
 
   std::vector<PathWithLaneId> partial_paths_;
   PathWithLaneId full_path_;
@@ -126,8 +128,9 @@ class PullOverPlannerBase
 
   virtual PullOverPlannerType getPlannerType() const = 0;
   virtual std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) = 0;
 
 protected:
   const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp

@@ -36,8 +36,9 @@ class ShiftPullOver : public PullOverPlannerBase
     const LaneDepartureChecker & lane_departure_checker);
   PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; };
   std::optional<PullOverPath> plan(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
-    const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override;
+    const GoalCandidate & modified_goal_pose, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
+    const BehaviorModuleOutput & previous_module_output) override;
 
 protected:
   PathWithLaneId generateReferencePath(
@@ -46,10 +47,10 @@ class ShiftPullOver : public PullOverPlannerBase
   std::optional<PathWithLaneId> cropPrevModulePath(
     const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const;
   std::optional<PullOverPath> generatePullOverPath(
-    const size_t goal_id, const size_t id, const std::shared_ptr<const PlannerData> planner_data,
+    const GoalCandidate & goal_candidate, const size_t id,
+    const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose,
-    const double lateral_jerk) const;
+    const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
   static std::vector<double> splineTwoPoints(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp

@@ -105,22 +105,12 @@ class ThreadSafeData
     return true;
   }
 
-  std::optional<PullOverPlannerType> getPullOverPlannerType() const
-  {
-    const std::lock_guard<std::recursive_mutex> lock(mutex_);
-    if (!pull_over_path_) {
-      return std::nullopt;
-    }
-    return pull_over_path_->type();
-  };
-
   void reset()
   {
     const std::lock_guard<std::recursive_mutex> lock(mutex_);
     pull_over_path_ = nullptr;
     pull_over_path_candidates_.clear();
     goal_candidates_.clear();
-    modified_goal_pose_ = std::nullopt;
     last_path_update_time_ = std::nullopt;
     last_path_idx_increment_time_ = std::nullopt;
     closest_start_pose_ = std::nullopt;
@@ -135,7 +125,6 @@ class ThreadSafeData
 
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector<PullOverPath>, pull_over_path_candidates)
   DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates)
-  DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<GoalCandidate>, modified_goal_pose)
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<Pose>, closest_start_pose)
   DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional<BehaviorModuleOutput>, last_previous_module_output)
   DEFINE_SETTER_GETTER_WITH_MUTEX(
@@ -168,7 +157,6 @@ class ThreadSafeData
   void set_no_lock(const std::vector<PullOverPath> & arg) { pull_over_path_candidates_ = arg; }
   void set_no_lock(const std::shared_ptr<PullOverPath> & arg) { set_pull_over_path_no_lock(arg); }
   void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); }
-  void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; }
   void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; }
   void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg)
   {
@@ -179,7 +167,6 @@ class ThreadSafeData
   std::shared_ptr<PullOverPath> lane_parking_pull_over_path_{nullptr};
   std::vector<PullOverPath> pull_over_path_candidates_;
   GoalCandidates goal_candidates_{};
-  std::optional<GoalCandidate> modified_goal_pose_;
   std::optional<rclcpp::Time> last_path_update_time_;
   std::optional<rclcpp::Time> last_path_idx_increment_time_;
   std::optional<Pose> closest_start_pose_{};

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -303,8 +303,7 @@
                                     const std::shared_ptr<PullOverPlannerBase> & planner,
                                     const GoalCandidate & goal_candidate) {
     const auto pull_over_path = planner->plan(
-      goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output,
-      goal_candidate.goal_pose);
+      goal_candidate, path_candidates.size(), local_planner_data, previous_module_output);
     if (pull_over_path) {
       // calculate absolute maximum curvature of parking path(start pose to end pose) for path
       // priority
@@ -544,14 +543,21 @@
     found_pull_over_path ? std::make_optional<PullOverPath>(*thread_safe_data_.get_pull_over_path())
                          : std::nullopt;
 
+  const auto modified_goal_pose = [&]() -> std::optional<GoalCandidate> {
+    if (!pull_over_path_recv) {
+      return std::nullopt;
+    }
+    const auto & pull_over_path = pull_over_path_recv.value();
+    return pull_over_path.modified_goal();
+  }();
+
   const bool is_current_safe = isSafePath(
     planner_data_, found_pull_over_path, pull_over_path_recv, *parameters_,
     ego_predicted_path_params_, objects_filtering_params_, safety_check_params_);
   path_decision_controller_.transit_state(
     found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects,
-    thread_safe_data_.get_modified_goal_pose(), planner_data_, occupancy_grid_map_, is_current_safe,
-    *parameters_, goal_searcher_, isActivated(), pull_over_path_recv,
-    debug_data_.ego_polygons_expanded);
+    modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_,
+    goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded);
 
   context_data_.emplace(
     path_decision_controller_.get_current_state().is_stable_safe, static_target_objects,
@@ -803,17 +809,15 @@
       continue;
     }
     const auto freespace_path = freespace_planner_->plan(
-      goal_candidate.id, 0, planner_data,
-      BehaviorModuleOutput{},  // NOTE: not used so passing {} is OK
-      goal_candidate.goal_pose);
+      goal_candidate, 0, planner_data, BehaviorModuleOutput{}  // NOTE: not used so passing {} is OK
+    );
     if (!freespace_path) {
       continue;
     }
 
     {
       const std::lock_guard<std::recursive_mutex> lock(mutex_);
       thread_safe_data_.set_pull_over_path(*freespace_path);
-      thread_safe_data_.set_modified_goal_pose(goal_candidate);
       debug_data_.freespace_planner.is_planning = false;
     }
 
@@ -907,7 +911,7 @@
   return fixed_goal_planner_->plan(planner_data_);
 }
 
-std::optional<std::pair<PullOverPath, GoalCandidate>> GoalPlannerModule::selectPullOverPath(
+std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
   const PullOverContextData & context_data,
   const std::vector<PullOverPath> & pull_over_path_candidates,
   const GoalCandidates & goal_candidates) const
@@ -1132,7 +1136,7 @@
       checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) {
       continue;
     }
-    return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id())));
+    return path;
   }
   return {};
 }
@@ -1229,7 +1233,7 @@
   if (context_data.pull_over_path_opt) {
     PoseWithUuidStamped modified_goal{};
     modified_goal.uuid = route_handler->getRouteUuid();
-    modified_goal.pose = thread_safe_data_.get_modified_goal_pose()->goal_pose;
+    modified_goal.pose = context_data.pull_over_path_opt.value().modified_goal_pose();
     modified_goal.header = route_handler->getRouteHeader();
     output.modified_goal = modified_goal;
   } else {
@@ -1371,55 +1375,50 @@
 
     // Select a path that is as safe as possible and has a high priority.
     const auto & pull_over_path_candidates = context_data.pull_over_path_candidates;
-    const auto path_and_goal_opt =
+    auto path_and_goal_opt =
       selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates);
 
     // update thread_safe_data_
     if (path_and_goal_opt) {
-      auto [pull_over_path, modified_goal] = *path_and_goal_opt;
+      auto pull_over_path = path_and_goal_opt.value();
       /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old
        * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed
        * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of
        * member variable
        *
        * set this selected pull_over_path to ThreadSafeData, but actually RoadParking thread does
        * not use pull_over_path, but only FreespaceParking thread use this selected pull_over_path.
        * As the next action item, only set this selected pull_over_path to only
        * FreespaceThreadSafeData.
        */
-      thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal);
+      thread_safe_data_.set(goal_candidates, pull_over_path);
       if (pull_over_path_with_velocity_opt) {
         auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value();
         // copy the path for later setOutput()
         pull_over_path_with_velocity = pull_over_path;
         // modify the velocity for latest setOutput()
         deceleratePath(pull_over_path_with_velocity);
       }
       RCLCPP_DEBUG(
         getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(),
-        modified_goal.id);
+        pull_over_path.modified_goal().id);
     } else {
       thread_safe_data_.set(goal_candidates);
     }
   }
 
   // set output and status
   BehaviorModuleOutput output{};
   setOutput(context_data_with_velocity, output);
 
   // return to lane parking if it is possible
   if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }
 
   // For debug
   setDebugData(context_data_with_velocity);
 
-  if (parameters_->print_debug_info) {
-    // For evaluations
-    printParkingPositionError();
-  }
-
   if (!pull_over_path_with_velocity_opt) {
     return output;
   }
@@ -1458,8 +1457,7 @@
   }
 
   updateSteeringFactor(
-    context_data,
-    {pull_over_path.start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose},
+    context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()},
     {distance_to_path_change.first, distance_to_path_change.second},
     has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING);
 
@@ -1511,10 +1509,10 @@
     full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx,
     pull_over_path.start_pose().position, start_pose_segment_idx);
   const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex(
-    full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position);
+    full_path.points, pull_over_path.modified_goal_pose().position);
   const double dist_to_parking_finish_pose = calcSignedArcLength(
     full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx,
-    thread_safe_data_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx);
+    pull_over_path.modified_goal_pose().position, goal_pose_segment_idx);
 
   return {dist_to_parking_start_pose, dist_to_parking_finish_pose};
 }
@@ -1810,7 +1808,7 @@
 
   const auto & current_pose = planner_data_->self_odometry->pose.pose;
   const auto & start_pose = pull_over_path.start_pose();
-  const auto & end_pose = pull_over_path.end_pose();
+  const auto & end_pose = pull_over_path.modified_goal_pose();
 
   const auto shift_start_idx =
     autoware::motion_utils::findNearestIndex(path.points, start_pose.position);
@@ -1824,7 +1822,7 @@
     return ignore_signal_.value() == id;
   };
 
-  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+  const auto update_ignore_signal = [](const lanelet::Id & id, const bool is_ignore) {
     return is_ignore ? std::make_optional(id) : std::nullopt;
   };
 
@@ -2393,114 +2391,115 @@
     const auto & pull_over_path = context_data.pull_over_path_opt.value();
     add(
       createPoseMarkerArray(pull_over_path.start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9));
-    add(createPoseMarkerArray(pull_over_path.end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9));
+    add(createPoseMarkerArray(
+      pull_over_path.modified_goal_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9));
     add(createPathMarkerArray(pull_over_path.full_path(), "full_path", 0, 0.0, 0.5, 0.9));
     add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0));
 
     // visualize each partial path
     for (size_t i = 0; i < context_data.pull_over_path_opt.value().partial_paths().size(); ++i) {
       const auto & partial_path = context_data.pull_over_path_opt.value().partial_paths().at(i);
       add(
         createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9));
     }
 
     auto marker = autoware::universe_utils::createDefaultMarker(
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST,
       autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0),
       autoware::universe_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999));
     const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
     for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) {
       for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) {
         const auto & current_point = ego_polygon.outer().at(ep_idx);
         const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size());
 
         marker.points.push_back(
           autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z));
         marker.points.push_back(
           autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z));
       }
     }
     debug_marker_.markers.push_back(marker);
 
     if (parameters_->safety_check_params.enable_safety_check) {
       autoware::universe_utils::appendMarkerArray(
         goal_planner_utils::createLaneletPolygonMarkerArray(
           debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header,
           "expanded_pull_over_lane_between_ego",
           autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)),
         &debug_marker_);
     }
 
     // Visualize debug poses
     const auto & debug_poses = pull_over_path.debug_poses;
     for (size_t i = 0; i < debug_poses.size(); ++i) {
       add(createPoseMarkerArray(
         debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3));
     }
   }
 
   auto collision_check = thread_safe_data_.get_collision_check();
   // safety check
   if (parameters_->safety_check_params.enable_safety_check) {
     if (goal_planner_data_.ego_predicted_path.size() > 0) {
       const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath(
         goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution);
       add(createPredictedPathMarkerArray(
         ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9));
     }
     if (goal_planner_data_.filtered_objects.objects.size() > 0) {
       add(createObjectsMarkerArray(
         goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
     }
 
     if (parameters_->safety_check_params.method == "RSS") {
       add(showSafetyCheckInfo(collision_check, "object_debug_info"));
     }
     add(showPredictedPath(collision_check, "ego_predicted_path"));
     add(showPolygon(collision_check, "ego_and_target_polygon_relation"));
 
     // set objects of interest
     for (const auto & [uuid, data] : collision_check) {
       const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
       setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
     }
 
     // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared
     utils::parking_departure::initializeCollisionCheckDebugMap(collision_check);
 
     // visualize safety status maker
     {
       const auto prev_data = thread_safe_data_.get_prev_data();
       visualization_msgs::msg::MarkerArray marker_array{};
       const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99)
                                                   : createMarkerColor(1.0, 0.0, 0.0, 0.99);
       auto marker = createDefaultMarker(
         header.frame_id, header.stamp, "safety_status", 0,
         visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color);
 
       marker.pose = planner_data_->self_odometry->pose.pose;
       marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n";
       if (prev_data.safe_start_time) {
         const double elapsed_time_from_safe_start =
           (clock_->now() - prev_data.safe_start_time.value()).seconds();
         marker.text +=
           "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n";
       }
       marker_array.markers.push_back(marker);
       add(marker_array);
     }
   }
 
   // Visualize planner type text
   {
     visualization_msgs::msg::MarkerArray planner_type_marker_array{};
     const auto color = context_data.pull_over_path_opt ? createMarkerColor(1.0, 1.0, 1.0, 0.99)
                                                        : createMarkerColor(1.0, 0.0, 0.0, 0.99);
     auto marker = createDefaultMarker(
       header.frame_id, header.stamp, "planner_type", 0,
       visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color);
-    marker.pose = thread_safe_data_.get_modified_goal_pose()
-                    ? thread_safe_data_.get_modified_goal_pose()->goal_pose
+    marker.pose = context_data.pull_over_path_opt
+                    ? context_data.pull_over_path_opt.value().modified_goal_pose()
                     : planner_data_->self_odometry->pose.pose;
     if (context_data.pull_over_path_opt) {
       const auto & pull_over_path = context_data.pull_over_path_opt.value();
@@ -2546,25 +2545,6 @@
   }
 }
 
-void GoalPlannerModule::printParkingPositionError() const
-{
-  const auto current_pose = planner_data_->self_odometry->pose.pose;
-  const double real_shoulder_to_map_shoulder = 0.0;
-
-  const Pose goal_to_ego =
-    inverseTransformPose(current_pose, thread_safe_data_.get_modified_goal_pose()->goal_pose);
-  const double dx = goal_to_ego.position.x;
-  const double dy = goal_to_ego.position.y;
-  const double distance_from_real_shoulder =
-    real_shoulder_to_map_shoulder + parameters_->margin_from_boundary - dy;
-  RCLCPP_INFO(
-    getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy,
-    autoware::universe_utils::rad2deg(
-      tf2::getYaw(current_pose.orientation) -
-      tf2::getYaw(thread_safe_data_.get_modified_goal_pose()->goal_pose.orientation)),
-    distance_from_real_shoulder);
-}
-
 bool GoalPlannerModule::needPathUpdate(
   const Pose & current_pose, const double path_update_duration,
   const GoalPlannerParameters & parameters) const