New planner

[paul-nechifor]

• If you want to see the navigation map in Foxglove use this:

DEBUG_NAVIGATION=1 dimos --robot-ip 192.168.x.xxx run unitree-go2-test-nav

• Added ability to generate a mujoco room from an occupancy map grid:

 dimos --mujoco-start-pos '4,2' --mujoco-camera-position "4.13,0,4.9,7.14,89,-50" --mujoco-room-from-occupancy data/occupancy_simple.npy run unitree-go2-test-nav

• Added ability to pre-load the global map from a previously saved pointcloud:

dimos --mujoco-start-pos '1.64,9.4' --mujoco-camera-position "4.13,0,4.9,7.14,89,-50" --mujoco-room-from-occupancy data/three_paths.png --mujoco-global-map-from-pointcloud data/three_paths.ply run unitree-go2-test-nav

I've fixed a bug with voxels being rounded down instead of to the nearest pixel. This produced these kinds of artifacts which were covered up by filling the empty space and inflating the occupied areas.

before

after

I've introduced another style for rendering occupancy maps. It renders fully occupied as magenta so it's easier to tell apart from unknown. This is the style used in tests. It doesn't have to be shown to the user.

turbo

rainbow

I've changed the A* algorithm to prioritize minimum cost above path length.

before

after

I've added a new function to resample A* paths. It produces much smoother paths (the robot is represented by a 0.2x0.4 rectangle at each pose):

before

after

[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 introduces a new navigation planner with significant improvements to path planning quality and performance. The implementation prioritizes minimum cost over path length, producing smoother, safer paths that stay farther from obstacles.

Key Changes:

• Implemented new min-cost A* algorithm that prioritizes cumulative cell cost over path length, with both Python and optimized C++ implementations
• Fixed voxel rounding bug (floor → nearest) that caused occupancy map artifacts
• Added smooth path resampling using moving average filters for reduced jagged corners
• Refactored occupancy mapping into modular components (inflation.py, gradient.py, operations.py, path_map.py)
• Added MuJoCo scene generation from occupancy grids for improved simulation testing
• Introduced configurable navigation strategies (simple/mixed) via GlobalConfig
• Extracted from_pointcloud and inflate methods from OccupancyGrid into dedicated modules for better separation of concerns

Issues Found:

• Duplicate import in dimos/navigation/global_planner/astar.py line 17 (syntax error)

The changes are well-tested with comprehensive unit tests and visual regression tests for all new algorithms.

Confidence Score: 4/5

• Safe to merge after fixing the duplicate import syntax error
• The PR introduces substantial improvements to navigation planning with thorough testing. The code quality is high with proper C++ optimization, comprehensive unit tests, and visual regression tests. The duplicate import is a minor syntax issue that must be fixed. The refactoring is well-structured and maintains backward compatibility.
• dimos/navigation/global_planner/astar.py requires fixing the duplicate import before merging

Important Files Changed

Filename	Overview
dimos/navigation/global_planner/min_cost_astar.py	introduces new min-cost A* algorithm prioritizing cumulative cell cost over path length, includes both Python and C++ implementations
dimos/navigation/global_planner/min_cost_astar_cpp.cpp	C++ implementation of min-cost A* for performance optimization, uses pybind11 for Python binding
dimos/navigation/global_planner/astar.py	unified A* interface with algorithm selection, has duplicate import on line 17
dimos/navigation/global_planner/planner.py	refactored to use new navigation map strategy and path resampling, integrated with GlobalConfig
dimos/mapping/occupancy/path_resampling.py	new module with simple and smooth path resampling methods, includes orientation calculation
dimos/mapping/pointclouds/occupancy.py	extracted from OccupancyGrid, implements two strategies (general_occupancy and simple_occupancy) with rounding fix from floor to nearest
dimos/mapping/occupancy/path_map.py	new module providing make_navigation_map with configurable strategies (simple/mixed) for costmap generation
dimos/core/global_config.py	extended with new config options: robot_width, planner_strategy, astar_algorithm, MuJoCo scene loading options

Sequence Diagram

sequenceDiagram
    participant User
    participant AstarPlanner
    participant NavigationMap
    participant MinCostAstar
    participant MinCostAstarCPP
    participant PathResampling

    User->>AstarPlanner: goal (PoseStamped)
    AstarPlanner->>AstarPlanner: check costmap & odom available
    
    AstarPlanner->>NavigationMap: make_navigation_map(costmap, robot_width, strategy)
    NavigationMap->>NavigationMap: apply inflation
    NavigationMap->>NavigationMap: apply gradient/voronoi
    NavigationMap-->>AstarPlanner: processed costmap
    
    AstarPlanner->>MinCostAstar: astar(algorithm, costmap, goal, start)
    
    alt use_cpp=True and C++ available
        MinCostAstar->>MinCostAstarCPP: min_cost_astar_cpp(grid, start, goal)
        MinCostAstarCPP->>MinCostAstarCPP: priority queue search (cost-first)
        MinCostAstarCPP-->>MinCostAstar: path coordinates
    else use Python fallback
        MinCostAstar->>MinCostAstar: Python A* (cost-first priority)
    end
    
    MinCostAstar-->>AstarPlanner: raw Path
    
    AstarPlanner->>PathResampling: simple_resample_path(path, goal_pose, spacing)
    PathResampling->>PathResampling: resample at uniform spacing
    PathResampling->>PathResampling: add orientations
    PathResampling-->>AstarPlanner: resampled Path
    
    AstarPlanner->>User: publish(path)

Loading

[greptile-apps]

Additional Comments (1)

1. dimos/msgs/nav_msgs/OccupancyGrid.py, line 189 (link)

   syntax: useless computation - result is never assigned or used

_{19 files reviewed, 2 comments}

_{Edit Code Review Agent Settings | Greptile}

[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]

_{72 files reviewed, 1 comment}

_{Edit Code Review Agent Settings | Greptile}

[leshy]

just quickly leaving some comments, still going in detail through replanning_a_star dir

[leshy]

do you think we should move all these into some planner/ dir or something? I assume we either need most or none

[paul-nechifor]

Hm, I'm not sure. There are quite a few things which need to be moved in there and doing it in this PR would make it even bigger than it already is. I'll add this on my todo list.

[leshy]

Just a note, I personally like to emit to foxglove and let it overlay stuff, idk if you thought of it or not

[leshy]

it's so easy to use tf, let's obsolete this

self.tf.get('world', 'base_link') 

[paul-nechifor]

Yep, I'll do this.

[paul-nechifor]

tf doesn't have subscribe. How would I subscribe to the latest position?

[leshy]

there is also dimos.spec (used by rosnav.py)

I also see you have .target and .set_goal etc, what's the reason for it? could we try and solidify the spec?

[paul-nechifor]

NavigationInterface contains the methods that are used in the agent skills. The skills module binds to NavigationInterface.set_goal etc to communicate with whichever planner is deployed.

Yeah, I'll look at unifying.

[leshy]

hmmm not a fan of centralized config that's expanded like this in a single place, "core" shouldn't care about your particular "planner_strategy"

we have a standard for shared config that comes with Configurable, overlayed on top of ModuleConfig, that also applies defaults.

dimos/dimos/mapping/voxels.py

Line 47 in 0c64608

class Config(ModuleConfig):

For global config we should be able to easily apply a deep dict, like

{
"memory_limit": ..., 
"n_dask_workers": ...

# this is class name
"astar_navigator": { 
    "strategy": ...
}
}

then if we want convenient CLI we can play with some sort of deep matching etc, but for a start --astar_navigator_strategy=... is also ok?

[paul-nechifor]

Most of these are meant to serve as a sort of feature flags: added as a way to test which is working better and removed once we're certain which is best.

The issue with a nested config is where to place the value. In this example, planner_strategy is used by make_navigation_map which is used by both navigation/global_planner/planner.py and navigation/replanning_a_star/navigation_map.py. So I'm not sure where to put it in this particular case.

I'm okay with using a nested config, though. Do you think it would be better if you added the code to read/pass the config? I'd do it myself, but I'm sensing a slight difference of opinion here (for example on yaml too) so it would be cool if I could see what you have in mind. 😃

[leshy]

aha this is important so let's make sure that I understand your perspective first also, don't want to trample over it if I don't understand it :D :D

[leshy]

had a talk, implementing a config system in a separate PR

[leshy]

as long as we are aware we have a rich visual language on fg (likely mapping to similar primitives in rerun) but still prefer this for now, all good.

dimos/dimos/perception/detection/type/detection3d/imageDetections3DPC.py

Line 26 in 0c64608

def to_foxglove_scene_update(self) -> SceneUpdate:

[leshy]

def nvm on this since rerun is coming