Planner improvements

[paul-nechifor]

• Separate out PD controller from local planner.
• Add P controller for sim.
• Allow unknown goals (does not try to make them safe).
• Reset retry attempts if traveled far enough (2 meters from where it was initially stuck).
• Added numba optimizations for occupancy. simple_occupancy went from 0.01057 to 0.00166. height_cost_occupancy went from 0.04679 to 0.01817.
• Changed /debug_navigation to only publish every second to not overwhelm the system.
• Merged the end-to-end navigation test in Mujoco ( End-to-end test: spatial memory navigation in MuJoCo #775 ) in here since navigation is so reliable now.

[chatgpt-codex-connector]

Codex usage limits have been reached for code reviews. Please check with the admins of this repo to increase the limits by adding credits.
Credits must be used to enable repository wide code reviews.

[greptile-apps]

Greptile Summary

This PR refactors the navigation system by extracting controller logic into separate classes and improving replanning robustness.

Key Changes:

• Controller separation: Extracted PD controller logic from LocalPlanner into a new controllers.py module with PController (for simulation) and PdController (for hardware). The P controller uses proportional control only, while PD adds derivative damping for smoother hardware control.
• Unknown goal support: Modified _find_safe_goal to skip safety validation when goal is in UNKNOWN cells, allowing navigation to unexplored areas
• Retry reset logic: Added ReplanLimiter class that resets retry attempts when the robot travels ≥2 meters from where it initially got stuck, preventing premature abandonment of valid goals
• Code cleanup: Replaced normalize_angle with new angle_diff utility, removed unused morphological closing from occupancy mapping, added cell_value helper to OccupancyGrid

Minor Issues:

• Four new config parameters (can_pass_under, can_climb, ignore_noise, smoothing) added to SimpleOccupancyConfig but not used anywhere in the codebase

Confidence Score: 5/5

• Safe to merge with high confidence
• Well-structured refactoring with proper separation of concerns. The controller extraction improves testability and maintainability. Unknown goal handling is a straightforward conditional check. Retry reset logic is thread-safe and addresses a real navigation issue. Test coverage added for new utilities.
• No files require special attention

Important Files Changed

Filename	Overview
dimos/navigation/replanning_a_star/controllers.py	New file extracting PD controller logic from local planner, adds P controller for simulation
dimos/navigation/replanning_a_star/local_planner.py	Refactored to use controller abstraction, simplified by delegating velocity control logic
dimos/navigation/replanning_a_star/global_planner.py	Integrated ReplanLimiter, added support for unknown goals by skipping safety validation
dimos/navigation/replanning_a_star/replan_limiter.py	New class tracking retry attempts with distance-based reset (2m threshold)
dimos/utils/trigonometry.py	New utility function for computing signed angle differences with proper wrapping
dimos/msgs/nav_msgs/OccupancyGrid.py	Added cell_value method for querying grid values at world positions

Sequence Diagram

sequenceDiagram
    participant User
    participant GlobalPlanner
    participant ReplanLimiter
    participant LocalPlanner
    participant Controller
    participant NavigationMap
    
    User->>GlobalPlanner: handle_goal_request(goal)
    GlobalPlanner->>ReplanLimiter: reset()
    GlobalPlanner->>GlobalPlanner: _plan_path()
    GlobalPlanner->>GlobalPlanner: _find_safe_goal(goal)
    alt Goal in UNKNOWN cell
        GlobalPlanner->>GlobalPlanner: return goal (skip safety check)
    else Goal needs validation
        GlobalPlanner->>NavigationMap: find_safe_goal()
    end
    GlobalPlanner->>GlobalPlanner: _find_wide_path()
    GlobalPlanner->>LocalPlanner: start_planning(path)
    
    loop Navigation Loop
        LocalPlanner->>Controller: advance(lookahead, odom)
        Controller->>Controller: compute yaw_error
        alt Simulation
            Controller->>Controller: PController (P control)
        else Hardware
            Controller->>Controller: PdController (PD control)
        end
        Controller-->>LocalPlanner: Twist command
        LocalPlanner->>User: publish cmd_vel
        
        alt Obstacle detected
            LocalPlanner->>GlobalPlanner: stopped_navigating("obstacle_found")
            GlobalPlanner->>ReplanLimiter: can_retry(position)
            alt Traveled >= 2m from stuck position
                ReplanLimiter->>ReplanLimiter: reset attempt counter
            end
            alt Attempts < max_attempts
                ReplanLimiter-->>GlobalPlanner: true
                GlobalPlanner->>GlobalPlanner: _replan_path()
            else Too many attempts
                ReplanLimiter-->>GlobalPlanner: false
                GlobalPlanner->>GlobalPlanner: cancel_goal()
            end
        end
        
        alt Robot veered off path
            GlobalPlanner->>GlobalPlanner: _replan_path()
        end
        
        alt Robot stuck (no state change)
            GlobalPlanner->>GlobalPlanner: _replan_path()
        end
        
        alt Goal reached
            LocalPlanner->>GlobalPlanner: stopped_navigating("arrived")
            GlobalPlanner->>User: goal_reached
        end
    end

Loading

[greptile-apps]

_{16 files reviewed, 1 comment}

_{Edit Code Review Agent Settings | Greptile}

[greptile-apps]

style: new config parameters added but not used anywhere in the code

Suggested change

	can_pass_under: float = 0.6
	can_climb: float = 0.15
	ignore_noise: float = 0.05
	smoothing: float = 1.0

[leshy]

Vector3 supports angle diffs, just noting, should add Vector2 eventually

[leshy]

merged via #887