fix(planning,control): stabilize reverse behavior and decouple front-distance reflex#124
Open
xiaolefang-dm wants to merge 13 commits intomainfrom
Open
fix(planning,control): stabilize reverse behavior and decouple front-distance reflex#124xiaolefang-dm wants to merge 13 commits intomainfrom
xiaolefang-dm wants to merge 13 commits intomainfrom
Conversation
xiaolefang-dm
force-pushed
the
xiaole/optimized/planning
branch
from
7baa340 to
ff67ae9
Compare
|
|
|
|
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>
…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>
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
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>
Removed redundant path publishing when target_pose is None or all trajectories are in collision.
Keep planner forward-only and delegate reverse handling to cmd_vel, while adding front-obstacle reflex and simple rotate-first gating to reduce risky turn-and-go behavior. Made-with: Cursor
Drop front_dist publisher coupling and keep reverse selection fully inside planning cost with single reverse candidate and front-clearance threshold gating. Made-with: Cursor
Normalize EOF newlines for planning_node.py and cmd_vel_control.py. Made-with: Cursor
Clean up unresolved conflict marker that caused ruff syntax failure in CI. Made-with: Cursor
…amline velocity sampling Added a new function to generate predefined trajectory vocabularies, allowing for future behaviors to be easily appended. Simplified the velocity sampling in the trajectory generation by removing the reverse sample and directly using a linear space for forward velocities. Updated trajectory generation in the PlanningNode to incorporate the new vocabulary, enhancing maneuvering capabilities.
xiaolefang-dm
force-pushed
the
xiaole/optimized/planning
branch
from
59f7d8f to
a7a9e12
Compare
|
…fy comments Updated the cost function in PlanningNode to improve clarity and maintainability. Introduced clearer comments regarding predefined backward trajectory penalties and streamlined the logic for determining when to apply reverse penalties. This refactor aims to enhance the understanding of trajectory evaluations and improve the overall planning logic.
|
|
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
cmd_vel_controlbehavior by keeping rotate-first gating for forward motion and enforcing straight-line reverse (vyaw=0when reversing)