Jeff/feat/g1 raycast ivan#2348
Conversation
Renames uintree_g1_primitive_no_nav.py to unitree_g1_primitive_no_nav.py, adds unitree_g1_onboard.py at the corrected path, updates the variable name and all importers, and regenerates the all_blueprints.py registry keys (drops the typo'd uintree-g1-primitive-no-nav entry, adds the correctly-spelled unitree-g1-onboard and unitree-g1-primitive-no-nav).
…does navigate but with problems)
Prefix with _ so it is not registered as a standalone runnable blueprint; it stays a shared composition for nav-simple and nav-onboard. unitree-g1-nav-simple is the only new blueprint.
# Conflicts: # dimos/mapping/ray_tracing/rust/.gitignore
leshy
requested review from
arkluc,
mustafab0,
paul-nechifor and
spomichter
as code owners
Codecov Report❌ Patch coverage is 📢 Thoughts on this report? Let us know! |
Greptile SummaryThis PR introduces a new
Confidence Score: 3/5The PR is safe to merge for most of its changes, but the The new dimos/mapping/costmapper.py (the _apply_initial_safe_radius method) and dimos/mapping/ray_tracing/rust/flake.lock (stale branch reference) need attention before or shortly after merge. Important Files Changed
Flowchart%%{init: {'theme': 'neutral'}}%%
flowchart TD
subgraph _unitree_g1_onboard
FastLio2 -->|global_map_fastlio| GL_F[global_map_fastlio stream]
FastLio2 -->|odometry| OdomStream[odometry stream]
G1DDS[G1HighLevelDdsSdk]
unitree_g1_vis[unitree_g1_vis]
end
subgraph unitree_g1_nav_simple
_unitree_g1_onboard
OdomStream --> RayTrace[RayTracingVoxelMap]
FastLio2 -->|lidar| RayTrace
RayTrace -->|global_map PointCloud2| CostMapper
CostMapper -->|global_costmap OccupancyGrid| AStarPlanner[ReplanningAStarPlanner]
OdomStream --> AStarPlanner
AStarPlanner -->|nav_cmd_vel Twist| MovementManager
AStarPlanner -->|path Path| unitree_g1_vis
CostMapper -->|global_costmap| unitree_g1_vis
MovementManager -->|cmd_vel Twist| G1DDS
end
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| def _apply_initial_safe_radius(self, grid: OccupancyGrid) -> None: | ||
| radius_meters = self.config.initial_safe_radius_meters | ||
| if radius_meters <= 0 or grid.grid.size == 0: | ||
| return | ||
|
|
||
| resolution = grid.resolution | ||
| origin_x = grid.origin.position.x | ||
| origin_y = grid.origin.position.y | ||
|
|
||
| rows, columns = np.ogrid[: grid.grid.shape[0], : grid.grid.shape[1]] | ||
| cell_world_x = columns * resolution + origin_x | ||
| cell_world_y = rows * resolution + origin_y | ||
| distance_squared_meters = cell_world_x**2 + cell_world_y**2 | ||
|
|
||
| # Half-cell tolerance: a cell counts as inside if any part of it overlaps | ||
| # the disc. Avoids floating-point boundary flakiness from radius/resolution. | ||
| effective_radius_meters = radius_meters + resolution * 0.5 | ||
| safe_mask = distance_squared_meters <= effective_radius_meters**2 | ||
| grid.grid[safe_mask] = 0 |
There was a problem hiding this comment.
_apply_initial_safe_radius computes each cell's distance from the world origin (0, 0) rather than from the robot's current position. The occupancy grid's origin is set to (min_x, min_y) of the global-frame point cloud, so cell_world_x = columns * resolution + origin_x gives the cell's position in the world frame. Squaring and summing those values gives distance-squared from (0, 0), not from wherever the robot actually is. Once the robot has moved any meaningful distance from its start point, the safe disc sits at the map origin rather than under the robot. CostMapper would need an odometry input to know the robot's current position and compute the correct offset.
| self.register_disposable(Disposable(self.odom.subscribe(self._planner.handle_odom))) | ||
| self.register_disposable( | ||
| Disposable( | ||
| self.odometry.subscribe( | ||
| lambda msg: self._planner.handle_odom(msg.to_pose_stamped()) | ||
| ) | ||
| ) | ||
| ) |
There was a problem hiding this comment.
Both odom (In[PoseStamped]) and odometry (In[Odometry]) are unconditionally subscribed, and both call self._planner.handle_odom. Any blueprint that wires up both streams will drive handle_odom twice per cycle. Guarding each subscription on the transport being non-None (as is done for stop_movement) or making them mutually exclusive would prevent this.
2 participants
No description provided.