Skip to content

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

Merged
dvorak0 merged 9 commits intomainfrom
fix/planning-goal-nav-stop
Apr 29, 2026
Merged

fix(planning): stop robot properly when no active nav goal #118
dvorak0 merged 9 commits intomainfrom
fix/planning-goal-nav-stop

Conversation

@xiaolefang-dm
Copy link
Copy Markdown
Contributor

@xiaolefang-dm xiaolefang-dm commented Apr 28, 2026

  • stop publishing path on goal-reached,
  • fix camera-frame distance check,
  • remove poi_changed dependency,
  • raise goal threshold to 0.5 m,
  • clean up dead-band logic in cmd_vel

@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 28, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.14 s (301.3 files/s, 44057.4 lines/s)
Language files blank comment code
Python 12 584 260 3243
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 41 905 358 4733

@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 28, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.17 s (239.8 files/s, 35024.0 lines/s)
Language files blank comment code
Python 12 583 259 3238
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 41 904 357 4728

@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 28, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.17 s (234.4 files/s, 34297.7 lines/s)
Language files blank comment code
Python 12 584 260 3247
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 41 905 358 4737

@xiaolefang-dm xiaolefang-dm force-pushed the fix/planning-goal-nav-stop branch from 291d405 to 85cb9d6 Compare April 28, 2026 06:16
@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 28, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.12 s (329.7 files/s, 48166.3 lines/s)
Language files blank comment code
Python 12 583 259 3238
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 41 904 357 4728

xiaolefang and others added 6 commits April 28, 2026 14:19
@claude
fix(planning): stop publishing path when robot reaches goal (< 0.3m)
2273af5 
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 <noreply@anthropic.com>
@claude
fix(planning): compare camera position to target_pose for goal-reache…
f423967 
…d 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 <noreply@anthropic.com>
@claude
fix(nav): stop robot properly when no active nav target
db9224e 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
delete poichanged in plannning
ebc1660
change dist_to_goal
6da1fd0
@claude
refactor(planning): remove redundant goal-reached check
e0e6a09 
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 <noreply@anthropic.com>
@xiaolefang-dm xiaolefang-dm force-pushed the fix/planning-goal-nav-stop branch from 85cb9d6 to e0e6a09 Compare April 28, 2026 06:19
@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 28, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.18 s (233.1 files/s, 34031.7 lines/s)
Language files blank comment code
Python 12 583 257 3237
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 41 904 355 4727

dvorak0
dvorak0 requested changes Apr 28, 2026
View reviewed changes
Comment thread tinynav/core/planning_node.py Outdated
@xiaolefang-dm
Remove unnecessary path publishing in planning_node
eac0394 
Removed redundant path publishing when target_pose is None or all trajectories are in collision.
@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 29, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.17 s (251.6 files/s, 36661.1 lines/s)
Language files blank comment code
Python 13 609 259 3342
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 31 30 196
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 42 930 357 4832

@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 29, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.17 s (251.5 files/s, 36716.5 lines/s)
Language files blank comment code
Python 13 609 259 3361
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 30 29 191
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 42 929 356 4846

@github-actions
Copy link
Copy Markdown

github-actions Bot commented Apr 29, 2026

cloc github.com/AlDanial/cloc v 2.04 T=0.17 s (253.4 files/s, 36990.8 lines/s)
Language files blank comment code
Python 13 609 259 3361
Markdown 4 155 6 426
C++ 4 60 38 409
Bourne Shell 15 41 9 272
Dockerfile 1 30 29 191
JSON 2 8 0 58
TOML 1 4 0 57
make 1 18 15 55
CMake 1 4 0 17
-------- -------- -------- -------- --------
SUM: 42 929 356 4846

@dvorak0 dvorak0 merged commit 45a905a into main Apr 29, 2026
6 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Reviewers

@dvorak0 dvorak0 dvorak0 requested changes

@junlinp junlinp Awaiting requested review from junlinp

Assignees

No one assigned

Labels

None yet

Projects

None yet

Milestone

No milestone

Development

Successfully merging this pull request may close these issues.

2 participants

@xiaolefang-dm @dvorak0