diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py
index 5e5b191..c3237c3 100644
--- a/tinynav/core/planning_node.py
+++ b/tinynav/core/planning_node.py
@@ -374,12 +374,8 @@ def __init__(self):
         self.target_pose = None
 
         self.poi_change_sub = self.create_subscription(Odometry, "/mapping/poi_change", self.poi_change_callback, 10)
-        self.poi_changed = False
-        self.poi_change_timestamp_sec = 0.0
 
     def poi_change_callback(self, msg):
-        self.poi_changed = True
-        self.poi_change_timestamp_sec = msg.header.stamp.sec
         self.target_pose = None
 
     def target_pose_callback(self, msg):
@@ -584,22 +580,21 @@ def cost_function(traj, param, score, target_pose):
             top_indices = np.argsort(np.array([cost_function(trajectories[i], params[i], scores[i], self.target_pose) for i in range(len(trajectories))]), kind='stable')[:top_k]
             self.last_param = params[top_indices[0]]
 
-            if self.poi_changed and (depth_msg.header.stamp.sec - self.poi_change_timestamp_sec) > 3.0:
-                self.poi_changed = False
-
             # path
             path = Path()
             path.header = depth_msg.header
             path.header.frame_id = "world"
+
+            if self.target_pose is None:
+                return
+
+            if all(s == float('inf') for s in scores):
+                self.get_logger().info('All trajectories in collision, stopping path.')
+                return
+
             for i in top_indices:
                 for j in range(0, len(trajectories[i]), 10):
                     x,y,z,qx,qy,qz,qw = trajectories[i][j]
-                    if self.poi_changed or self.target_pose is None:
-                        x,y,z,qx,qy,qz,qw = trajectories[i][0]
-                        if self.poi_changed:
-                            self.get_logger().info(f"poi changed, using first point, wait {(depth_msg.header.stamp.sec - self.poi_change_timestamp_sec)} seconds")
-                        if self.target_pose is None:
-                            self.get_logger().info("target pose is None, using first point")
 
                     pose = PoseStamped()
                     pose.header = depth_msg.header