Skip to content

feat(humanoid): G1 gmapping 室内建图导航 - #1274

Merged
LinFengCi merged 2 commits into
OpenHUTB:masterfrom
usernamekkyy:feat/g1_gmapping_navigation
Nov 12, 2026
Merged

LinFengCi merged 2 commits into
OpenHUTB:masterfrom
usernamekkyy:feat/g1_gmapping_navigation

Conversation

@usernamekkyy

Copy link
Copy Markdown
Contributor

修改目的

通过仿真雷达和里程计为 G1 同步建图,并用 move_base 执行地图目标。

这是 #1085 中拆出的独立人形机器人示例。各模块可在上游基线上独立运行,仿真资产按 README 中固定版本获取。

验证

  • 从 main.py 启动 ROS Noetic,检查 /map、/odom、相机与关节话题,并通过 move_base 执行地图目标。
  • 已用 mkdocs serve --livereload 检查首页、导航及模块页面。
  • 环境、运行步骤及启动参数见 src/humanoid/g1_gmapping_navigation/README.md。

运行效果

G1 gmapping 室内建图导航

大模型使用声明

使用 OpenAI Codex 辅助拆分、编程、验证和文档整理。提交者复核并对内容负责。

通过仿真雷达和里程计为 G1 同步建图,并用 move_base 执行地图目标。
@Donk-666-lcx

Copy link
Copy Markdown
Contributor

先说明我这边能做到什么程度:这个 PR 我一行都没跑起来——它需要 unitree_rl_gym 资产(git clone 上游仓库)、MuJoCo 3.2 的 GL 渲染、ROS Noetic + gmapping/move_base,PR 里也没有任何离线测试。所以下面的意见全部来自读代码 + 局部导入验证,不是"跑出来的",跟 #1277 那种能 python tests/... 直接跑的情况不一样。

我也试了最小化导入:

$ python -c "import sys; sys.path.insert(0,'.'); import patrol.scene"
ModuleNotFoundError: No module named 'mujoco'
$ python -c "import sys; sys.path.insert(0,'.'); import patrol.locomotion"
ModuleNotFoundError: No module named 'mujoco'

patrol/scene.py 顶部的 import mujoco / from .locomotion import Walker 把整个模块绑死在 MuJoCo + torch 上,可 build_scene() 本身只用到 xml.etree 和 numpy,完全可以在没有 MuJoCo 的机器上单测。这也是我下面第 7 条建议的出发点。

具体问题

1. scripts/ros_bridge.py 第 122–125 行注释与数据源不符

注释写 "Velocity from successive integrated odometry is in the body frame",但 patrol/bridge_server.py 第 50 行给的是:

velocity=[float(sim.data.qvel[0]), float(sim.data.qvel[1])]

MuJoCo 自由关节的 qvel[0:2] 是世界系线速度(scene.py:67 里 IndoorSimulator.step() 也是先 R^T @ qvel[:2] 才转成机体系)。也就是说 state['velocity'] 是世界系,而 ros_bridge.py 又乘了一次 [[cos,sin],[-sin,cos]] 把它转回机体系——代码是对的,注释写反了。但这条注释很危险:后来的人照着注释"修",就会把这次变换删掉,odom.twist 立刻变成世界系速度。建议改成"世界系速度 → 机体系 twist"。

2. front_camera 没有 TF

ros_bridge.py:141 发 image.header.frame_id='front_camera',但 TF 只发了 odom→base_link 和 base_link→laser。rviz 把 fixed frame 设成 map 时图像会因为缺少 front_camera 直接报 "No transform from [front_camera]"。而 scene.py:21 里相机名字是 front(挂在 pelvis 上,pos='0.16 0 0.35'),名字也不一致。建议补一条静态变换,名字统一成 front_camera。

3. launch/main.launch 里的 teleop 分支不可达

<group unless="$(eval arg('task') == 'teleop')">

但 main.py 的 --mode 只有 navigation / bridge,task arg 默认 navigation,README/run_navigation.sh 也不会传 teleop。这个条件恒为真,等于给读代码的人留了个"是不是有手柄遥控模式"的假线索。建议删掉,或把 teleop 路径真正补上。

4. CMakeLists.txt 里 config 写了两遍

install(DIRECTORY launch config config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

无害,但明显是复制粘贴漏改,建议删一个。

5. patrol/scene.py:16 没判空

root.find('compiler').set('meshdir', ...)

上游 g1_12dof.xml 哪天没有 <compiler> 节点,这里就是 AttributeError: 'NoneType' object has no attribute 'set',报错完全指不到真实原因。建议显式判空并抛出说明。

6. 每帧 180 次单点 mj_ray

scan() 是纯 Python 循环:5 Hz × 180 条射线 × 每条一次 mj_ray。现在能跑,但换成 360 线或提高频率会立刻成为瓶颈,建议用 mujoco.mj_multiRay 一次发一批。

7. 建议让 build_scene() 可以脱开 MuJoCo 单测

把 import mujoco 和 from .locomotion import Walker 移到 IndoorSimulator 内部(或用 if TYPE_CHECKING),build_scene() 就能在纯 Python 环境里被验证:生成的 XML 能被 xml.etree 解析、worldbody 里有 floor/walls/door_*/inspection_marker、compiler@meshdir 被改写、camera@name='front' 存在。这类断言不需要渲染、不需要资产,CI 里几十行就能守住——现在这个模块最大的问题是"作者跑过一次,别人无法复现"。

8. ROS 版本

package.xml 是 format="2" + catkin + rospy,launch 是 ROS 1 XML,依赖 gmapping / move_base(这两个都是 Noetic 的包),而仓库是 OpenHUTB/ros2。如果这是有意的 ROS 1 兼容(比如为了复用 unitree_rl_gym 的 deploy 脚本),建议在 README 里明确写"本模块为 ROS 1 Noetic";否则 ROS 2 生态里对应的是 slam_toolbox + nav2,合并进来会变成仓库里一套起不来的包。

另外 scripts/fetch_assets.py 把上游 revision 固定成 276801e4...,这个做法很好;不过 git clone --filter=blob:none 没加 --depth,全历史仍然会拉下来,建议补 --depth 1 再 checkout --detach。

@usernamekkyy 1、2 两条建议优先处理(都会让人在 RViz 里直接踩坑),4、5 是几行的小修。如果愿意,我可以按第 7 条提一个只加 tests/test_scene_xml_local.py 的改动给你们参考。

@usernamekkyy

Copy link
Copy Markdown
Contributor Author

@xingwunian

@usernamekkyy

Copy link
Copy Markdown
Contributor Author

回应 这条评审,修复已推送到 08b865d8。

优先修复了第 1、2 条。注释明确区分 MuJoCo 世界系线速度和 ROS base_link 下的 twist,保留原有坐标变换。相机统一使用 front_camera,根据实际相机姿态发布同一图像时间戳下的光学 TF,涵盖 G1 的俯仰与摇摆。

清理了无效的 teleop 分支、launch 参数及重复的 CMake 安装项。场景 XML 生成现在可单独导入,有无 compiler 节点均有离线测试。180 条雷达射线改用一次批量查询,实测与逐条查询的距离一致。资产脚本按固定提交浅拉取,README 已注明 ROS 1 Noetic。

本地用固定版本资产和 MuJoCo 3.2.0 复测,按图像时间戳可查询 base_link 到 front_camera 的 TF,地图更新 23 次,三个 move_base 目标均成功。

@LinFengCi LinFengCi left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

审核通过,批准合并。

  • 文件结构合规:源码在 src/humanoid/g1_gmapping_navigation/,文档在 docs/humanoid/g1_gmapping_navigation/,文件名全英文下划线,入口 main.py + launch/main.launch,导航栏(mkdocs.yml)与首页(docs/index.md)均已加跳转链接。
  • 运行环境与步骤在 README 中说明清楚(Python 3.11、依赖安装、fetch_assets 获取 G1 资产、main.py 运行与 ROS Noetic 启动)。
  • 大模型使用声明完整:明确“使用 OpenAI Codex 辅助……提交者复核并对提交内容负责”。
  • 运行效果图 slam_map.png 已附(<3MB),为真实生成图;附带第三方许可说明(third_party_license/notices.md),合规。
  • 说明:本环境无 ROS/MuJoCo 与 G1 仿真资产,无法在此完整复现 ROS 启动;已对全部 Python 完成语法编译、核对结构/命名/声明/导航,并依据作者自测结论与仓库规范通过。请另一名同学在另一台机器按 README 复跑确认。

@LinFengCi
LinFengCi merged commit f20ef87 into OpenHUTB:master Nov 12, 2026
2 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants