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: 5/5Safe to merge; changes are well-scoped refactors and additions with no data-loss or crash-inducing paths introduced. The core logic changes (deadzone compensation, frame_id propagation, Odometry conversion, A* frame fix) are all contained and correct. The new nav stack composes existing tested modules. No issues were found that would cause incorrect navigation behavior or data corruption on the changed paths. Hardware deployment of unitree_g1_nav_simple warrants manual verification of the increased speed (0.85 m/s) and the deadzone boost interaction, particularly for lateral velocity commands on the G1. Important Files Changed
Sequence DiagramsequenceDiagram
participant Lidar as FastLio2 (LiDAR SLAM)
participant DDS as G1HighLevelDdsSdk
participant RT as RayTracingVoxelMap
participant CM as CostMapper
participant RAP as ReplanningAStarPlanner
participant MM as MovementManager
Lidar->>RT: global_map_fastlio (PointCloud2)
Lidar->>RAP: odometry (Odometry to_pose_stamped)
RT->>CM: voxel_map (PointCloud2)
CM->>CM: _apply_initial_safe_radius()
CM->>RAP: global_costmap (OccupancyGrid)
RAP->>RAP: "A* replan with costmap frame_id"
RAP->>MM: path (Path)
MM->>DDS: cmd_vel (Twist)
DDS->>DDS: _boost_above_deadzone(vx, vy, vyaw)
DDS->>DDS: loco_client.move(vx, vy, vyaw)
Reviews (2): Last reviewed commit: "Auto-fixes for jeff/feat/g1_raycast_ivan..." | Re-trigger Greptile |
![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.
3 participants
No description provided.