Branch: Feat/ivan/m20 Optimized the adaptation of simple_nav for M20#2807
Open
MeloLong wants to merge 3 commits into
Open
Branch: Feat/ivan/m20 Optimized the adaptation of simple_nav for M20#2807MeloLong wants to merge 3 commits into
MeloLong wants to merge 3 commits into
Conversation
MeloLong
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Contributor
Greptile SummaryThis PR adapts the
Confidence Score: 4/5Safe to merge after fixing the M20TF odometry topic remapping in basic.py. The dimos/robot/deeprobotics/m20/blueprints/basic.py — the M20TF remapping line needs the same Important Files Changed
Flowchart%%{init: {'theme': 'neutral'}}%%
flowchart TD
Bridge["zenoh_lcm_bridge\n(publishes dimos/slam_*)"]
M20Conn["M20Connection\n(video + cmd_vel)"]
M20TF["M20TF\nremapped to slam_odom WARNING\n(should be dimos/slam_odom)"]
RayTracer["_m20_slam_ray_tracer\nremapped to dimos/slam_odom OK"]
Occupancy["CostMapper / HeightCostConfig\nignore_overhead_only=True\nmin_gradient_neighbors=2"]
AStar["ReplanningAStarPlanner\nremapped to dimos/slam_odom OK"]
PCtrl["PController\nM20: min_angular=0.6 rad/s"]
MM["MovementManager"]
Robot["M20 Robot"]
Bridge -->|dimos/slam_aligned_points| RayTracer
Bridge -->|dimos/slam_odom| RayTracer
Bridge -.->|slam_odom no match| M20TF
Bridge -->|dimos/slam_odom| AStar
M20Conn -->|color_image / status| Rerun["RerunBridgeModule"]
RayTracer -->|local_map| Occupancy
Occupancy -->|costmap| AStar
AStar --> PCtrl
PCtrl --> MM
MM -->|cmd_vel| M20Conn
M20Conn --> Robot
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
flowchart TD
Bridge["zenoh_lcm_bridge\n(publishes dimos/slam_*)"]
M20Conn["M20Connection\n(video + cmd_vel)"]
M20TF["M20TF\nremapped to slam_odom WARNING\n(should be dimos/slam_odom)"]
RayTracer["_m20_slam_ray_tracer\nremapped to dimos/slam_odom OK"]
Occupancy["CostMapper / HeightCostConfig\nignore_overhead_only=True\nmin_gradient_neighbors=2"]
AStar["ReplanningAStarPlanner\nremapped to dimos/slam_odom OK"]
PCtrl["PController\nM20: min_angular=0.6 rad/s"]
MM["MovementManager"]
Robot["M20 Robot"]
Bridge -->|dimos/slam_aligned_points| RayTracer
Bridge -->|dimos/slam_odom| RayTracer
Bridge -.->|slam_odom no match| M20TF
Bridge -->|dimos/slam_odom| AStar
M20Conn -->|color_image / status| Rerun["RerunBridgeModule"]
RayTracer -->|local_map| Occupancy
Occupancy -->|costmap| AStar
AStar --> PCtrl
PCtrl --> MM
MM -->|cmd_vel| M20Conn
M20Conn --> Robot
Reviews (2): Last reviewed commit: "Remove accidentally committed result sym..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
Comment on lines
+53
to
+54
| if global_config.robot_model == "m20": | ||
| self._min_angular_velocity = 0.6 |
Contributor
There was a problem hiding this comment.
When robot_model is m20, the controller clips angular speed to self._speed before applying this new 0.6 minimum. The default planner speed is 0.55, so any non-zero heading error becomes ±0.6 rad/s, including tiny corrections while driving forward; the M20 path follower can zig-zag or overshoot instead of making proportional yaw corrections.
Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
| can_climb=m20_max_step_height, | ||
| ignore_noise=0.08, | ||
| smoothing=1.5, | ||
| min_gradient_neighbors=2, |
Contributor
There was a problem hiding this comment.
This M20 config accepts a gradient when only 2 of the center/N/S/E/W cells are observed. In sparse SLAM clouds, a boundary cell with mostly unknown neighbors now receives a finite cost from smoothed or zero-filled heights instead of staying unknown, so A* can prefer it as known traversable ground and route the robot through poorly observed obstacles or drop-offs.
Comment on lines
+20
to
+23
| #include <limits> | ||
| #include <mutex> | ||
| #include <string> | ||
| #include <vector> |
Contributor
There was a problem hiding this comment.
This new native target uses std::cout and std::endl, but the file does not include <iostream>. Building m20_pointcloud_map_save will fail as soon as this module is enabled or compiled directly.
Suggested change
| #include <limits> | |
| #include <mutex> | |
| #include <string> | |
| #include <vector> | |
| #include <iostream> | |
| #include <limits> | |
| #include <mutex> | |
| #include <string> | |
| #include <vector> |
| @@ -0,0 +1 @@ | |||
| /nix/store/nk8n7m2h8lcanp77aypjsq1pa6xs2fh0-smartnav-terrain-analysis-0.1.0 No newline at end of file | |||
Contributor
There was a problem hiding this comment.
This commits a result symlink to an absolute /nix/store/... path. On a fresh checkout, CI runner, or another robot without that exact store path, the link is dangling and any terrain-analysis build or runtime path that expects this artifact will fail with a missing file.
|
|
||
| # Deepwork local progress state | ||
| .slim/deepwork/ | ||
| data/* |
Contributor
There was a problem hiding this comment.
We already have /data/* in the file. But data/.lfs should not be ignored.
Some of the files you're ignoring here are already committed. Also you have duplicates.
| ] | ||
| ) | ||
|
|
||
| # _m20_pointcloud_map_save = PointCloudMapSave.blueprint( |
Contributor
There was a problem hiding this comment.
delete commented code
❌ 4 Tests Failed:
View the top 3 failed test(s) by shortest run time
To view more test analytics, go to the Test Analytics Dashboard |
| global_emit_every=10, | ||
| # M20's onboard SLAM publishes this cloud already registered in map frame. | ||
| registered_clouds=True, | ||
| ).remappings( |
Author
There was a problem hiding this comment.
Because the LiDAR and odometry topics published by the M20 are not named lidar and odometry, but as shown in the current code.
| from dimos.msgs.geometry_msgs.Twist import Twist | ||
|
|
||
|
|
||
| def main() -> None: |
Contributor
There was a problem hiding this comment.
What is this for? If you just need to publish the same Twist, you can do it from the command line: uv run dimos topic send /nav/cmd_vel "Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0))"
Author
There was a problem hiding this comment.
Sure, I'll remove it. It was just some test code from when I first started working on this.
|
|
||
| # Verify protocol port compliance (mypy will flag missing ports) | ||
| if TYPE_CHECKING: | ||
| PointCloudMapSave() |
Contributor
There was a problem hiding this comment.
Why create and discard this?
Author
There was a problem hiding this comment.
I'll remove this code. It's a deprecated feature.
| @@ -0,0 +1 @@ | |||
| /nix/store/nk8n7m2h8lcanp77aypjsq1pa6xs2fh0-smartnav-terrain-analysis-0.1.0 No newline at end of file | |||
Contributor
There was a problem hiding this comment.
I assume this was accidentally commited?
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
(1) Introduced local ground height estimation to suppress the influence of elevated structures on costmap generation.
A local ground height is first estimated. If a grid cell only contains points that are significantly higher than the estimated ground, those points are treated as ceiling or elevated structures and ignored, instead of being directly used to generate the traversal cost. This prevents elevated points from being mistakenly classified as ground bumps or obstacles, thereby avoiding the incorrect removal of traversable areas.
(2) Relaxed the ground continuity constraint to improve robustness under sparse point cloud observations.
The original method required a grid cell and all four of its neighboring cells (front, back, left, and right) to have valid observations before slope and traversability costs could be computed reliably. Under sparse point clouds or local ground gaps, this strict requirement caused many valid ground cells to be marked as unknown. We therefore relaxed the criterion from complete neighborhood observation to a sufficient number of valid neighboring observations, reducing the spread of unknown regions caused by sparse measurements and improving the completeness of traversable areas.
Resolved the M20 velocity command response issue by enforcing the robot's minimum velocity response thresholds in the simple_nav trajectory planner. The minimum command limits are:
Linear velocity (x): ≥ 0.2 m/s
Linear velocity (y): ≥ 0.3 m/s
Angular velocity (yaw): ≥ 0.6 rad/s
When linear and angular motions are commanded simultaneously: yaw ≥ 0.2 rad/s
How to run:
Tips: m20 is a IP of m20 robot, please replace it.
DIMOS_ZENOH_CONNECT=tcp/m20_ip python -m dimos.robot.deeprobotics.m20.zenoh_lcm_bridgedimos run m20-simple-nav