diff --git a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp
index 2c23bd8..35aa81c 100644
--- a/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp
+++ b/navyu_path_planner/include/navyu_path_planner/navyu_global_planner.hpp
@@ -44,8 +44,6 @@ class NavyuGlobalPlanner : public rclcpp::Node
 
   void publish_path(std::vector<Node2D *> path);
 
-  void wait_for_costmap();
-
   bool get_robot_pose(geometry_msgs::msg::Pose & robot_pose);
 
 private:
@@ -63,8 +61,6 @@ class NavyuGlobalPlanner : public rclcpp::Node
 
   std::string map_frame_;
   std::string base_frame_;
-
-  bool costmap_initialized_{false};
 };
 
 #endif  // NAVYU_PLANNER__NAVYU_GLOBAL_PLANNER_HPP_
diff --git a/navyu_path_planner/src/navyu_global_planner.cpp b/navyu_path_planner/src/navyu_global_planner.cpp
index 234b540..4783d4d 100644
--- a/navyu_path_planner/src/navyu_global_planner.cpp
+++ b/navyu_path_planner/src/navyu_global_planner.cpp
@@ -45,15 +45,6 @@ NavyuGlobalPlanner::~NavyuGlobalPlanner()
 {
 }
 
-void NavyuGlobalPlanner::wait_for_costmap()
-{
-  rclcpp::WallRate rate(1);
-  while (!costmap_initialized_) {
-    RCLCPP_INFO_STREAM(get_logger(), "Wait For Costmap...");
-    rate.sleep();
-  }
-}
-
 bool NavyuGlobalPlanner::get_robot_pose(geometry_msgs::msg::Pose & robot_pose)
 {
   geometry_msgs::msg::TransformStamped robot_pose_frame;
@@ -139,9 +130,7 @@ void NavyuGlobalPlanner::callback_goal_pose(const geometry_msgs::msg::PoseStampe
 
 void NavyuGlobalPlanner::callback_costmap(const nav_msgs::msg::OccupancyGrid & msg)
 {
-  if (!costmap_initialized_) {
-    costmap_initialized_ = true;
-
+  if (planner_ == nullptr) {
     int size_x = msg.info.width;
     int size_y = msg.info.height;
     double resolution = msg.info.resolution;
@@ -159,11 +148,6 @@ void NavyuGlobalPlanner::callback_costmap(const nav_msgs::msg::OccupancyGrid & m
     return;
   }
 
-  if (planner_ == nullptr) {
-    RCLCPP_ERROR_STREAM(get_logger(), "Planner is not set.");
-    return;
-  }
-
   // set costmap info
   planner_->set_origin(msg.info.origin.position.x, msg.info.origin.position.y);
   planner_->set_size(msg.info.width, msg.info.height);