Revert to old nav

[paul-nechifor]

No description provided.

[greptile-apps]

Greptile Summary

This PR reverts PR #923 to restore the original navigation stack architecture. The revert brings back the three-layer navigation system: BehaviorTreeNavigator for high-level coordination, AstarPlanner for global path planning, and HolonomicLocalPlanner for local trajectory execution.

Key changes restored:

• Navigation coordinator: BehaviorTreeNavigator manages state machine, goal validation, stuck detection via RecoveryServer, and coordinates between global/local planners
• Global planning: AstarPlanner module with support for both general and min_cost A* algorithms, including C++ acceleration
• Local planning: BaseLocalPlanner abstract class and HolonomicLocalPlanner implementing GLAP (Gradient-Augmented Look-Ahead Pursuit) for holonomic movement
• Goal validation: Multiple algorithms (BFS, spiral, voronoi, gradient descent) to find safe goal positions near obstacles
• Configuration: Added astar_algorithm parameter to GlobalConfig for algorithm selection
• Integration: Updated blueprints to wire the old navigation stack and moved C++ extension path

The revert adds ~2000 lines of code across 26 files, restoring comprehensive test coverage and documentation.

Confidence Score: 4/5

• This PR is generally safe as it reverts to previously tested code with proper architecture
• Clean revert of a prior removal with well-structured navigation stack. Minor concerns: integration testing needed to verify nav stack interoperability with current mapper changes, and the stuck detection threshold (5s at line navigator.py:115) may need tuning
• Verify dimos/robot/unitree_webrtc/unitree_g1_blueprints.py blueprint wiring is compatible with the current mapper implementation

Important Files Changed

Filename	Overview
dimos/navigation/bt_navigator/navigator.py	Re-adds BehaviorTreeNavigator with goal validation, recovery server, and state machine for coordinating navigation
dimos/navigation/bt_navigator/goal_validator.py	Re-adds safe goal finding algorithms (BFS, spiral, voronoi, gradient descent) to handle goals near obstacles
dimos/navigation/global_planner/planner.py	Re-adds AstarPlanner module with navigation map generation and path resampling
dimos/navigation/local_planner/holonomic_local_planner.py	Re-adds GLAP holonomic planner combining pure pursuit with gradient-based obstacle repulsion
dimos/robot/unitree_webrtc/unitree_g1_blueprints.py	Updates blueprints to use old nav stack (astar_planner, holonomic_local_planner, behavior_tree_navigator)
setup.py	Updates C++ extension path from replanning_a_star to global_planner module

Sequence Diagram

sequenceDiagram
    participant User
    participant Navigator as BehaviorTreeNavigator
    participant GoalValidator as Goal Validator
    participant GlobalPlanner as AstarPlanner
    participant LocalPlanner as HolonomicLocalPlanner
    participant RecoveryServer
    participant Robot
    
    User->>Navigator: set_goal(PoseStamped)
    Navigator->>Navigator: Transform to odom frame
    Navigator->>Navigator: Set state to FOLLOWING_PATH
    
    loop Control Loop (1Hz)
        Navigator->>RecoveryServer: update_odom(latest_odom)
        RecoveryServer->>RecoveryServer: Check if robot stuck
        
        alt Robot is stuck
            RecoveryServer-->>Navigator: stuck detected
            Navigator->>Navigator: cancel_goal()
            Navigator->>LocalPlanner: reset()
        else Robot moving
            Navigator->>GoalValidator: find_safe_goal(costmap, goal)
            GoalValidator->>GoalValidator: Run BFS/spiral/voronoi/gradient
            GoalValidator-->>Navigator: safe_goal_position
            
            alt Safe goal found
                Navigator->>GlobalPlanner: publish target(safe_goal)
                GlobalPlanner->>GlobalPlanner: astar(costmap, goal, robot_pos)
                GlobalPlanner->>GlobalPlanner: resample_path(path)
                GlobalPlanner->>LocalPlanner: publish path
                
                LocalPlanner->>LocalPlanner: compute_path_following(path)
                LocalPlanner->>LocalPlanner: compute_obstacle_repulsion(costmap)
                LocalPlanner->>LocalPlanner: transform to robot frame
                LocalPlanner->>Robot: publish cmd_vel
                
                Navigator->>LocalPlanner: check_goal_reached()
                
                alt Goal reached
                    LocalPlanner-->>Navigator: true
                    Navigator->>User: publish goal_reached
                    Navigator->>Navigator: Set state to IDLE
                    Navigator->>LocalPlanner: reset()
                end
            else No safe goal
                Navigator->>Navigator: cancel_goal()
            end
        end
    end

Loading