From 2273af5b23d25457fedf160394d7f90533477d57 Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Mon, 27 Apr 2026 17:58:46 +0800 Subject: [PATCH 1/8] fix(planning): stop publishing path when robot reaches goal (< 0.3m) target_pose is never cleared on arrival, so planning_node kept generating paths and the robot never stopped. Now publishes an empty path once within 0.3m of target_pose so cmd_vel_control stale-path protection kicks in. Co-Authored-By: Claude Sonnet 4.6 --- tinynav/core/planning_node.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 5e5b191..8dfb881 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -591,6 +591,16 @@ def cost_function(traj, param, score, target_pose): path = Path() path.header = depth_msg.header path.header.frame_id = "world" + + # Goal-reached: publish empty path so cmd_vel stops. + if self.target_pose is not None: + robot_center = self.camera_to_robot_center(T) + dist_to_goal = float(np.linalg.norm(robot_center[:2] - self.target_pose[:2])) + if dist_to_goal < 0.3: + self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.', once=False) + self.path_pub.publish(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] From f4239676be9122fd4068f67a6aedbb45b57bb89c Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Mon, 27 Apr 2026 18:14:31 +0800 Subject: [PATCH 2/8] fix(planning): compare camera position to target_pose for goal-reached check target_pose is a camera-frame position (POI recorded as camera pose), but previously compared against robot control center which is 0.35m behind the camera, so dist_to_goal was always ~0.35m > 0.3m threshold. Now compare T[:3,3] (camera position) directly to target_pose. Co-Authored-By: Claude Sonnet 4.6 --- tinynav/core/planning_node.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 8dfb881..2227bda 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -593,11 +593,12 @@ def cost_function(traj, param, score, target_pose): path.header.frame_id = "world" # Goal-reached: publish empty path so cmd_vel stops. + # target_pose is a camera-frame position (POI was recorded as camera pose), + # so compare directly against camera position T[:3, 3]. if self.target_pose is not None: - robot_center = self.camera_to_robot_center(T) - dist_to_goal = float(np.linalg.norm(robot_center[:2] - self.target_pose[:2])) + dist_to_goal = float(np.linalg.norm(T[:3, 3][:2] - self.target_pose[:2])) if dist_to_goal < 0.3: - self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.', once=False) + self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.') self.path_pub.publish(path) return From db9224e09846fcbc6b0d1033b2c1ec1ca235f447 Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Tue, 28 Apr 2026 14:19:17 +0800 Subject: [PATCH 3/8] fix(nav): stop robot properly when no active nav target Co-Authored-By: Claude Sonnet 4.6 --- tinynav/core/planning_node.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 2227bda..e832b25 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -602,15 +602,16 @@ def cost_function(traj, param, score, target_pose): self.path_pub.publish(path) return + if self.target_pose is None and not self.poi_changed: + self.path_pub.publish(path) # empty path — no active nav target + 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: + if self.poi_changed: 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") + self.get_logger().info(f"poi changed, using first point, wait {(depth_msg.header.stamp.sec - self.poi_change_timestamp_sec)} seconds") pose = PoseStamped() pose.header = depth_msg.header From ebc1660799f75e882a406c5fec7acded1a113185 Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Tue, 28 Apr 2026 09:14:25 +0800 Subject: [PATCH 4/8] delete poichanged in plannning --- tinynav/core/planning_node.py | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index e832b25..d5c2731 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,9 +580,6 @@ 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 @@ -602,16 +595,18 @@ def cost_function(traj, param, score, target_pose): self.path_pub.publish(path) return - if self.target_pose is None and not self.poi_changed: - self.path_pub.publish(path) # empty path — no active nav target + if self.target_pose is None: + self.path_pub.publish(path) + return + + if all(s == float('inf') for s in scores): + self.get_logger().info('All trajectories in collision, stopping path.') + self.path_pub.publish(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: - x,y,z,qx,qy,qz,qw = trajectories[i][0] - self.get_logger().info(f"poi changed, using first point, wait {(depth_msg.header.stamp.sec - self.poi_change_timestamp_sec)} seconds") pose = PoseStamped() pose.header = depth_msg.header From 6da1fd0b75edbcffe3a505079faa0a7cd49cae49 Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Tue, 28 Apr 2026 09:16:10 +0800 Subject: [PATCH 5/8] change dist_to_goal --- tinynav/core/planning_node.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index d5c2731..42e11f3 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -585,15 +585,12 @@ def cost_function(traj, param, score, target_pose): path.header = depth_msg.header path.header.frame_id = "world" - # Goal-reached: publish empty path so cmd_vel stops. - # target_pose is a camera-frame position (POI was recorded as camera pose), - # so compare directly against camera position T[:3, 3]. - if self.target_pose is not None: - dist_to_goal = float(np.linalg.norm(T[:3, 3][:2] - self.target_pose[:2])) - if dist_to_goal < 0.3: - self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.') - self.path_pub.publish(path) - return + # target_pose is camera-frame (POI recorded as camera pose); compare against T[:3, 3] directly. + dist_to_goal = float(np.linalg.norm(T[:3, 3][:2] - self.target_pose[:2])) if self.target_pose is not None else float('inf') + if dist_to_goal < 0.5: + self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.') + self.path_pub.publish(path) + return if self.target_pose is None: self.path_pub.publish(path) From e0e6a091fa2be0ce6c1a67f06c7a9cb8a8ed4767 Mon Sep 17 00:00:00 2001 From: xiaolefang Date: Tue, 28 Apr 2026 13:55:07 +0800 Subject: [PATCH 6/8] refactor(planning): remove redundant goal-reached check map_node already detects arrival (dist < 0.5m) and publishes poi_change, which clears target_pose in planning_node, which then publishes an empty path to stop the robot. The dist_to_goal guard here is unreachable. Co-Authored-By: Claude Sonnet 4.6 --- tinynav/core/planning_node.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 42e11f3..9566159 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -585,13 +585,6 @@ def cost_function(traj, param, score, target_pose): path.header = depth_msg.header path.header.frame_id = "world" - # target_pose is camera-frame (POI recorded as camera pose); compare against T[:3, 3] directly. - dist_to_goal = float(np.linalg.norm(T[:3, 3][:2] - self.target_pose[:2])) if self.target_pose is not None else float('inf') - if dist_to_goal < 0.5: - self.get_logger().info(f'Goal reached (dist={dist_to_goal:.2f}m), stopping path.') - self.path_pub.publish(path) - return - if self.target_pose is None: self.path_pub.publish(path) return From eac039487277480c4d4fb50cd4f2d3b22e3c0c97 Mon Sep 17 00:00:00 2001 From: Xiaole Fang Date: Wed, 29 Apr 2026 08:01:18 +0800 Subject: [PATCH 7/8] Remove unnecessary path publishing in planning_node Removed redundant path publishing when target_pose is None or all trajectories are in collision. --- tinynav/core/planning_node.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 9566159..c3237c3 100644 --- a/tinynav/core/planning_node.py +++ b/tinynav/core/planning_node.py @@ -586,12 +586,10 @@ def cost_function(traj, param, score, target_pose): path.header.frame_id = "world" if self.target_pose is None: - self.path_pub.publish(path) return if all(s == float('inf') for s in scores): self.get_logger().info('All trajectories in collision, stopping path.') - self.path_pub.publish(path) return for i in top_indices: From 3a7900265b303e6b75cff37f5365df9b031e506c Mon Sep 17 00:00:00 2001 From: xiaolefang-dm Date: Wed, 29 Apr 2026 17:52:37 +0800 Subject: [PATCH 8/8] ci: trigger CI re-run after LFS quota restored