Skip to content

fix(planning): stop robot properly when no active nav goal #118

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
dvorak0 merged 9 commits into from
Apr 29, 2026
Merged

fix(planning): stop robot properly when no active nav goal #118

Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2273af5
fix(planning): stop publishing path when robot reaches goal (< 0.3m)
Apr 27, 2026
f423967
fix(planning): compare camera position to target_pose for goal-reache…
Apr 27, 2026
db9224e
fix(nav): stop robot properly when no active nav target
Apr 28, 2026
ebc1660
delete poichanged in plannning
Apr 28, 2026
6da1fd0
change dist_to_goal
Apr 28, 2026
e0e6a09
refactor(planning): remove redundant goal-reached check
Apr 28, 2026
eac0394
Remove unnecessary path publishing in planning_node
xiaolefang-dm Apr 29, 2026
3a79002
ci: trigger CI re-run after LFS quota restored
xiaolefang-dm Apr 29, 2026
ed653ec
Merge remote-tracking branch 'origin/main' into fix/planning-goal-nav…
xiaolefang-dm Apr 29, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 8 additions & 13 deletions tinynav/core/planning_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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
Expand Down
Loading