diff --git a/.github/workflows/ros2_rov_pipeline_follow.yml b/.github/workflows/ros2_rov_pipeline_follow.yml
index df98556677..7ae47a3341 100644
--- a/.github/workflows/ros2_rov_pipeline_follow.yml
+++ b/.github/workflows/ros2_rov_pipeline_follow.yml
@@ -37,7 +37,10 @@ jobs:
- name: colcon build
run: |
source /opt/ros/humble/setup.bash
- colcon build --packages-select rov_pipeline_follow --symlink-install
+ # The repository also has src/water/package.xml; start discovery at
+ # the nested package so colcon does not stop at the parent package.
+ colcon build --base-paths src/water/rov_pipeline_follow \
+ --packages-select rov_pipeline_follow --symlink-install
# ---- 3) install 布局硬断言:少一项就说明 data_files 又漏了
- name: 校验 install 布局
diff --git a/.gitignore b/.gitignore
index 4e193d427e..89337bc380 100644
--- a/.gitignore
+++ b/.gitignore
@@ -95,6 +95,13 @@ src/air/drl_drone_path_planning/models/train_summary.json
# 例外:Autoencoder 雷达修复模块的离线训练权重必须入库(约 96 KB,免重训)
!src/ground/lidar_autoencoder_repair/models/*.pt
+# 视觉通行模块的四个部署权重(约 17 KB、40 KB、61 KB、1.64 MB)
+!src/humanoid/humanoid_visual_patrol/models/perception.pt
+!src/humanoid/humanoid_visual_patrol/models/end2end.pt
+!src/humanoid/humanoid_visual_patrol/models/mask_controller.pt
+!src/humanoid/humanoid_visual_patrol/models/planner.pt
+src/humanoid/humanoid_visual_patrol/results/
+
# 例外:人形 H1/H1_2 行走模块的 TorchScript 策略必须入库(137/143 KB,免重训)
!src/humanoid/humanoid_h1_walk/weights/*.pt
diff --git a/docs/humanoid/humanoid_visual_patrol/evaluation.json b/docs/humanoid/humanoid_visual_patrol/evaluation.json
new file mode 100644
index 0000000000..e2ace2eab2
--- /dev/null
+++ b/docs/humanoid/humanoid_visual_patrol/evaluation.json
@@ -0,0 +1,1093 @@
+{
+ "end2end_7": {
+ "mode": "end2end",
+ "scene_seed": 7,
+ "simulated_seconds": 29.45999999999455,
+ "wall_seconds": 2.1050050258636475,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.500928685799143,
+ -0.4098442664875997,
+ 0.7759206563914889
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.012438045118886324
+ },
+ "end2end_8": {
+ "mode": "end2end",
+ "scene_seed": 8,
+ "simulated_seconds": 31.579999999993376,
+ "wall_seconds": 2.3076367378234863,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.503404718295236,
+ -1.0501545513112895,
+ 0.7742367789168679
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.03067057421028003
+ },
+ "locomotion_smoke": {
+ "simulated_seconds": 12.000000000000677,
+ "fallen": false,
+ "displacement_m": 2.287985076372512,
+ "min_height_m": 0.7661107569028477,
+ "final_yaw_rad": 0.7960921896645273,
+ "final_position": [
+ 2.272985425640397,
+ 0.26155872099716265,
+ 0.778752585346743
+ ]
+ },
+ "navigation_result": {
+ "server_ready": true,
+ "map_updates": 21,
+ "first_map": {
+ "stamp": 5.199999999,
+ "width": 288,
+ "height": 320,
+ "known": 11143,
+ "occupied": 352
+ },
+ "last_map": {
+ "stamp": 30.599999999,
+ "width": 288,
+ "height": 320,
+ "known": 18328,
+ "occupied": 1275
+ },
+ "goals": [
+ {
+ "goal": [
+ 2.0,
+ -0.3
+ ],
+ "finished": true,
+ "state": 3,
+ "odom": [
+ 1.6131431608545903,
+ -0.37145751631762564
+ ]
+ },
+ {
+ "goal": [
+ 4.0,
+ -0.3
+ ],
+ "finished": true,
+ "state": 3,
+ "odom": [
+ 3.6526522289419505,
+ -0.4557591917484762
+ ]
+ },
+ {
+ "goal": [
+ 7.0,
+ 0.0
+ ],
+ "finished": true,
+ "state": 3,
+ "odom": [
+ 6.73615725296174,
+ -0.2689369038850545
+ ]
+ }
+ ],
+ "camera": {
+ "width": 128,
+ "height": 96,
+ "bytes": 36864
+ },
+ "joint_count": 12
+ },
+ "neural_navigation_result": {
+ "waypoints": [
+ [
+ 1.5750000681728125,
+ -0.07499995641410351
+ ],
+ [
+ 3.0750000905245543,
+ -0.07499995641410351
+ ],
+ [
+ 4.575000112876296,
+ -0.07499995641410351
+ ],
+ [
+ 6.075000135228038,
+ -0.07499995641410351
+ ],
+ [
+ 7.0,
+ 0.0
+ ]
+ ],
+ "expanded": 47,
+ "baseline_expanded": 47,
+ "path_cost": 46,
+ "baseline_path_cost": 46,
+ "unknown_policy": "provisionally traversable; live local obstacle avoidance required",
+ "method": "Manhattan A* with neural equal-f tie breaking",
+ "request_id": 7799999999,
+ "execution": [
+ {
+ "goal": [
+ 1.5750000681728125,
+ -0.07499995641410351
+ ],
+ "state": 3
+ },
+ {
+ "goal": [
+ 3.0750000905245543,
+ -0.07499995641410351
+ ],
+ "state": 3
+ },
+ {
+ "goal": [
+ 4.575000112876296,
+ -0.07499995641410351
+ ],
+ "state": 3
+ },
+ {
+ "goal": [
+ 6.075000135228038,
+ -0.07499995641410351
+ ],
+ "state": 3
+ },
+ {
+ "goal": [
+ 7.0,
+ 0.0
+ ],
+ "state": 3
+ }
+ ],
+ "success": true
+ },
+ "perception_7": {
+ "mode": "perception",
+ "scene_seed": 7,
+ "simulated_seconds": 30.41999999999402,
+ "wall_seconds": 2.4995839595794678,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.503956924401511,
+ -1.1252807956239923,
+ 0.7736514554436829
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.014016393594219575
+ },
+ "perception_8": {
+ "mode": "perception",
+ "scene_seed": 8,
+ "simulated_seconds": 32.099999999993265,
+ "wall_seconds": 2.5015172958374023,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.503228414520496,
+ -0.6834990817441247,
+ 0.7741088336048865
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.027739362518145388
+ },
+ "teacher_7": {
+ "mode": "teacher",
+ "scene_seed": 7,
+ "simulated_seconds": 30.359999999994052,
+ "wall_seconds": 2.1397719383239746,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.500330321965385,
+ -1.2885485142101345,
+ 0.7769777181711114
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.014627753766444346
+ },
+ "teleop_check": {
+ "events": [
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 0.24000000000000019
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 0.4200000000000003
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 0.6400000000000005
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 0.8600000000000007
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.0400000000000007
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.2200000000000009
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.380000000000001
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.5800000000000012
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.7800000000000014
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.25,
+ 0.0
+ ],
+ "time": 1.9600000000000015
+ },
+ {
+ "key": 68,
+ "command": [
+ 0.0,
+ -0.35
+ ],
+ "time": 2.139999999999986
+ },
+ {
+ "key": 68,
+ "command": [
+ 0.0,
+ -0.35
+ ],
+ "time": 2.339999999999964
+ },
+ {
+ "key": 68,
+ "command": [
+ 0.0,
+ -0.35
+ ],
+ "time": 2.539999999999942
+ },
+ {
+ "key": 68,
+ "command": [
+ 0.0,
+ -0.35
+ ],
+ "time": 2.73999999999992
+ },
+ {
+ "key": 68,
+ "command": [
+ 0.0,
+ -0.35
+ ],
+ "time": 2.939999999999898
+ },
+ {
+ "key": 65,
+ "command": [
+ 0.0,
+ 0.35
+ ],
+ "time": 3.139999999999876
+ },
+ {
+ "key": 65,
+ "command": [
+ 0.0,
+ 0.35
+ ],
+ "time": 3.3399999999998538
+ },
+ {
+ "key": 65,
+ "command": [
+ 0.0,
+ 0.35
+ ],
+ "time": 3.519999999999834
+ },
+ {
+ "key": 65,
+ "command": [
+ 0.0,
+ 0.35
+ ],
+ "time": 3.699999999999814
+ },
+ {
+ "key": 65,
+ "command": [
+ 0.0,
+ 0.35
+ ],
+ "time": 3.899999999999792
+ },
+ {
+ "key": 83,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 4.0999999999997705
+ },
+ {
+ "key": 83,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 4.279999999999751
+ },
+ {
+ "key": 83,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 4.499999999999726
+ },
+ {
+ "key": 83,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 4.659999999999709
+ },
+ {
+ "key": 83,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 4.859999999999687
+ },
+ {
+ "key": 61,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 5.039999999999667
+ },
+ {
+ "key": 61,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 5.239999999999645
+ },
+ {
+ "key": 61,
+ "command": [
+ -0.25,
+ 0.0
+ ],
+ "time": 5.439999999999623
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.39999999999999997,
+ 0.0
+ ],
+ "time": 5.599999999999605
+ },
+ {
+ "key": 87,
+ "command": [
+ 0.39999999999999997,
+ 0.0
+ ],
+ "time": 5.799999999999583
+ },
+ {
+ "key": 45,
+ "command": [
+ 0.39999999999999997,
+ 0.0
+ ],
+ "time": 5.999999999999561
+ },
+ {
+ "key": 45,
+ "command": [
+ 0.39999999999999997,
+ 0.0
+ ],
+ "time": 6.199999999999539
+ },
+ {
+ "key": 45,
+ "command": [
+ 0.39999999999999997,
+ 0.0
+ ],
+ "time": 6.379999999999519
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 6.579999999999497
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 6.779999999999475
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 6.979999999999453
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 7.165999999999433
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 7.3599999999994115
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 7.559999999999389
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 7.73999999999937
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 7.939999999999348
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 8.139999999999388
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 8.319999999999448
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 8.519999999999515
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 8.719999999999581
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 8.879999999999635
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 9.079999999999702
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 9.299999999999775
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 9.479999999999835
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 9.659999999999895
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 9.879999999999969
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 10.060000000000029
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 10.260000000000096
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 10.440000000000156
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 10.640000000000223
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 10.860000000000296
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 11.060000000000363
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 11.26000000000043
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 11.460000000000496
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 11.660000000000563
+ },
+ {
+ "key": 32,
+ "command": [
+ 0.0,
+ 0.0
+ ],
+ "time": 11.880000000000637
+ }
+ ],
+ "final_position": [
+ 0.6022900755554784,
+ -0.24782982735759396,
+ 0.778801981361132
+ ],
+ "fallen": false,
+ "simulated_seconds": 12.000000000000677
+ },
+ "training": {
+ "perception": {
+ "test": {
+ "floor_iou": 0.7144569158554077
+ },
+ "history": [
+ {
+ "epoch": 1,
+ "train_loss": 0.6515593886375427,
+ "validation_loss": 0.5862979888916016
+ },
+ {
+ "epoch": 2,
+ "train_loss": 0.5322169105211894,
+ "validation_loss": 0.41666868329048157
+ },
+ {
+ "epoch": 3,
+ "train_loss": 0.3731572171052297,
+ "validation_loss": 0.31106099486351013
+ },
+ {
+ "epoch": 4,
+ "train_loss": 0.2773292029897372,
+ "validation_loss": 0.2671062648296356
+ },
+ {
+ "epoch": 5,
+ "train_loss": 0.2249623293677966,
+ "validation_loss": 0.23184753954410553
+ },
+ {
+ "epoch": 6,
+ "train_loss": 0.19123106648524602,
+ "validation_loss": 0.20464134216308594
+ },
+ {
+ "epoch": 7,
+ "train_loss": 0.1699126695593198,
+ "validation_loss": 0.1904342621564865
+ },
+ {
+ "epoch": 8,
+ "train_loss": 0.15063215494155885,
+ "validation_loss": 0.17608746886253357
+ },
+ {
+ "epoch": 9,
+ "train_loss": 0.13404261047641436,
+ "validation_loss": 0.1671636402606964
+ },
+ {
+ "epoch": 10,
+ "train_loss": 0.12165957614779473,
+ "validation_loss": 0.15130828320980072
+ },
+ {
+ "epoch": 11,
+ "train_loss": 0.10908799370129903,
+ "validation_loss": 0.13740822672843933
+ },
+ {
+ "epoch": 12,
+ "train_loss": 0.10004713286956152,
+ "validation_loss": 0.13193221390247345
+ },
+ {
+ "epoch": 13,
+ "train_loss": 0.09509985347588858,
+ "validation_loss": 0.12277023494243622
+ },
+ {
+ "epoch": 14,
+ "train_loss": 0.08908139442404112,
+ "validation_loss": 0.11898733675479889
+ },
+ {
+ "epoch": 15,
+ "train_loss": 0.08403473868966102,
+ "validation_loss": 0.1125747337937355
+ },
+ {
+ "epoch": 16,
+ "train_loss": 0.08217032079895338,
+ "validation_loss": 0.11868017166852951
+ },
+ {
+ "epoch": 17,
+ "train_loss": 0.07921109373370806,
+ "validation_loss": 0.105145663022995
+ },
+ {
+ "epoch": 18,
+ "train_loss": 0.07703785821795464,
+ "validation_loss": 0.10191230475902557
+ },
+ {
+ "epoch": 19,
+ "train_loss": 0.07324557105700175,
+ "validation_loss": 0.0991356372833252
+ },
+ {
+ "epoch": 20,
+ "train_loss": 0.07076147037247817,
+ "validation_loss": 0.10474512726068497
+ }
+ ],
+ "training_seconds": 18.795533418655396
+ },
+ "end2end": {
+ "test": {
+ "speed_mae_mps": 0.02873709797859192,
+ "yaw_rate_mae_radps": 0.07807976007461548
+ },
+ "history": [
+ {
+ "epoch": 1,
+ "train_loss": 0.019927604349019627,
+ "validation_loss": 0.015247656032443047
+ },
+ {
+ "epoch": 2,
+ "train_loss": 0.013338427625906963,
+ "validation_loss": 0.012845995835959911
+ },
+ {
+ "epoch": 3,
+ "train_loss": 0.009953329991549253,
+ "validation_loss": 0.010511630214750767
+ },
+ {
+ "epoch": 4,
+ "train_loss": 0.008802419520604114,
+ "validation_loss": 0.013564219698309898
+ },
+ {
+ "epoch": 5,
+ "train_loss": 0.007528269515993695,
+ "validation_loss": 0.009634070098400116
+ },
+ {
+ "epoch": 6,
+ "train_loss": 0.005717495433054864,
+ "validation_loss": 0.009699787944555283
+ },
+ {
+ "epoch": 7,
+ "train_loss": 0.004978025514477243,
+ "validation_loss": 0.008820238523185253
+ },
+ {
+ "epoch": 8,
+ "train_loss": 0.0043911014023857815,
+ "validation_loss": 0.009981256909668446
+ },
+ {
+ "epoch": 9,
+ "train_loss": 0.003916206141002476,
+ "validation_loss": 0.009927424602210522
+ },
+ {
+ "epoch": 10,
+ "train_loss": 0.004110249632503837,
+ "validation_loss": 0.010983279906213284
+ },
+ {
+ "epoch": 11,
+ "train_loss": 0.003298697933011378,
+ "validation_loss": 0.009419215843081474
+ },
+ {
+ "epoch": 12,
+ "train_loss": 0.0026648445830990872,
+ "validation_loss": 0.007939483039081097
+ },
+ {
+ "epoch": 13,
+ "train_loss": 0.0024159081862308086,
+ "validation_loss": 0.008272224105894566
+ },
+ {
+ "epoch": 14,
+ "train_loss": 0.002422941313125193,
+ "validation_loss": 0.00868251733481884
+ },
+ {
+ "epoch": 15,
+ "train_loss": 0.0019108354560254763,
+ "validation_loss": 0.008090991526842117
+ },
+ {
+ "epoch": 16,
+ "train_loss": 0.0017981273510182898,
+ "validation_loss": 0.008004545234143734
+ },
+ {
+ "epoch": 17,
+ "train_loss": 0.0016011812549550086,
+ "validation_loss": 0.009002798236906528
+ },
+ {
+ "epoch": 18,
+ "train_loss": 0.001546536212360176,
+ "validation_loss": 0.008609442971646786
+ },
+ {
+ "epoch": 19,
+ "train_loss": 0.0014207457948941737,
+ "validation_loss": 0.008610133081674576
+ },
+ {
+ "epoch": 20,
+ "train_loss": 0.0012522446893854066,
+ "validation_loss": 0.009489416144788265
+ }
+ ],
+ "training_seconds": 4.659215688705444
+ },
+ "mask_controller": {
+ "epochs": 100,
+ "validation_mse": 0.0027787438593804836,
+ "test_speed_mae_mps": 0.01168871857225895,
+ "test_yaw_rate_mae_radps": 0.048268433660268784
+ }
+ },
+ "planner_training": {
+ "train_maps": 240,
+ "validation_maps": 40,
+ "test_maps": 40,
+ "test_mae_cells": 0.7555620074272156,
+ "validation_mse": [
+ 0.07789694517850876,
+ 0.02313491329550743,
+ 0.011575024574995041,
+ 0.00888085551559925,
+ 0.006188783794641495,
+ 0.005088148172944784,
+ 0.004368642810732126,
+ 0.003886079415678978,
+ 0.0029025787953287363,
+ 0.0028561274521052837,
+ 0.0027689868584275246,
+ 0.00214683311060071,
+ 0.0019310235511511564,
+ 0.001588753191754222,
+ 0.001514676958322525,
+ 0.0014852900058031082,
+ 0.001386171323247254,
+ 0.0013086803955957294,
+ 0.0012405538000166416,
+ 0.0011569717898964882
+ ],
+ "scope": "synthetic occupancy maps; not visual training scenes"
+ },
+ "perception_9": {
+ "mode": "perception",
+ "scene_seed": 9,
+ "simulated_seconds": 31.439999999993454,
+ "wall_seconds": 4.430427312850952,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.5020529612010725,
+ -0.9344328852960936,
+ 0.7773054296349473
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.03544455567901597
+ },
+ "perception_10": {
+ "mode": "perception",
+ "scene_seed": 10,
+ "simulated_seconds": 30.41999999999402,
+ "wall_seconds": 3.4013195037841797,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.502263401781641,
+ -1.1874273503767911,
+ 0.7735630895421056
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.05658640509787874
+ },
+ "perception_11": {
+ "mode": "perception",
+ "scene_seed": 11,
+ "simulated_seconds": 30.819999999993797,
+ "wall_seconds": 3.7954225540161133,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.50444187888058,
+ -0.9793791840700157,
+ 0.7678394853311956
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.021790147097236014
+ },
+ "perception_12": {
+ "mode": "perception",
+ "scene_seed": 12,
+ "simulated_seconds": 30.699999999993864,
+ "wall_seconds": 3.201212167739868,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.501021051329815,
+ -1.1233923591479789,
+ 0.7770555982312694
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.04592932339707591
+ },
+ "end2end_9": {
+ "mode": "end2end",
+ "scene_seed": 9,
+ "simulated_seconds": 29.559999999994496,
+ "wall_seconds": 2.964218854904175,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.505344022030739,
+ -0.5925750182237367,
+ 0.7767791604811192
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.03564252525587454
+ },
+ "end2end_10": {
+ "mode": "end2end",
+ "scene_seed": 10,
+ "simulated_seconds": 29.99999999999425,
+ "wall_seconds": 15.170369863510132,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.502644446115033,
+ -0.21897350089130785,
+ 0.771064878556972
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.05675195153087234
+ },
+ "end2end_11": {
+ "mode": "end2end",
+ "scene_seed": 11,
+ "simulated_seconds": 30.619999999993908,
+ "wall_seconds": 3.099447011947632,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.504029707618278,
+ -0.7136290023136964,
+ 0.7776695657480839
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.026391199870822925
+ },
+ "end2end_12": {
+ "mode": "end2end",
+ "scene_seed": 12,
+ "simulated_seconds": 31.13999999999362,
+ "wall_seconds": 2.786536693572998,
+ "fallen": false,
+ "reached": true,
+ "final_position": [
+ 7.504242986277308,
+ -0.8676467751623445,
+ 0.7774885119970814
+ ],
+ "collision_steps": 0,
+ "odometry_error_m": 0.0451717146363518
+ }
+}
diff --git a/docs/humanoid/humanoid_visual_patrol/index.md b/docs/humanoid/humanoid_visual_patrol/index.md
new file mode 100644
index 0000000000..b571335855
--- /dev/null
+++ b/docs/humanoid/humanoid_visual_patrol/index.md
@@ -0,0 +1,118 @@
+# 人形机器人视觉通行感知与室内导航
+
+本示例在 MuJoCo 3.2.0 中驱动 Unitree G1,通过视觉通行识别和图像速度策略完成走廊通行,通过 ROS Noetic 完成同步建图导航。**当前为可复现原型,不是经过真实机器人验证的部署方案。**
+
+## 四项任务
+
+| 项目 | 实现 | 当前证据 |
+|---|---|---|
+| 键盘运动 | WASD/方向键、调速、超时停止,经官方步态输出关节力矩 | 真实 GLFW 窗口收到自动发送的前进/转向/停止按键,12秒未跌倒 |
+| 感知控制 | RGB 图像 → FloorNet → MaskController → 速度 | 两个留出场景闭环通过 |
+| 建图导航 | 模拟雷达+带噪积分里程计 → gmapping 与 move_base 同时运行 | 神经辅助规划生成的五个航点均返回 SUCCEEDED |
+| 端到端 | RGB 图像 → ImageController → 速度 → 步态底座 | 两个留出场景闭环通过 |
+
+任务二的感知和控制网络均经过训练;任务三采用经典 SLAM、神经网络辅助全局搜索及经典局部避障的混合方案,并非纯神经 SLAM 或纯神经规划。任务四学习图像到高层速度,并不直接输出全身关节力矩。
+
+## 运行效果
+
+以下为真实 MuJoCo 物理步进的离屏渲染,右上角是机载相机画面。
+
+### 视觉感知控制
+
+
+
+### 图像直接控制
+
+
+
+### SLAM 地图
+
+
+
+地图来自运行中的 `/map`,不是场景几何直接绘制。仿真真实位置仅用于离线评价;导航接收的是对带噪速度与航向增量积分的里程计。
+
+## 系统原理
+
+底层采用官方预训练 G1 12 腿部关节步态策略,固定上半身。物理频率 500 Hz,策略频率 50 Hz。观测包括角速度、重力投影、速度命令、关节位置/速度、前次动作及步态相位。策略输出关节位置残差,PD 控制产生力矩:
+
+$$\tau=K_p(q_{target}-q)-K_d\dot q,\qquad q_{target}=q_0+0.25a.$$
+
+底层预训练网络没有在本项目重新训练。独立训练的三个视觉相关模型如下:
+
+| 模型 | 输入 | 结构 | 输出与损失 |
+|---|---|---|---|
+| FloorNet | 96×128 RGB | 5×5 Conv(16)、3×3 Conv(16)、1×1 Conv(1) | 逐像素地面概率;BCE |
+| MaskController | 地面语义图 | 12×16池化、64/32维MLP | 前进速度、偏航角速度;MSE |
+| ImageController | 96×128 RGB | 三层步长2卷积+64维全连接 | 前进速度、偏航角速度;MSE |
+
+示范策略在图像中部至下部比较候选通行条带,对偏离中心的条带施加惩罚,并按前方通行像素比例限制速度。MaskController 学习此策略,任务二在线阶段使用学习后的网络输出速度。针对分割结果完全没有地面像素的情况,部署端增加规则保护并将速度和转向置零;保护不是网络能力,也不能处理所有误分割或障碍。当前测试用固定的无地面分割输出验证保护逻辑;真实 FloorNet 在空白图上的误分割仍未单独测试。它是局部反应式控制器,不等价于全局最短路规划。
+
+任务四以同一个视觉规则控制器产生示范标签,网络推理时只接收 RGB。两者形成“显式分割+语义控制网络”和“图像直接回归控制”的对照。速度限幅和缓动用于接口保护,不能掩盖模型失败。
+
+## 数据与训练
+
+MuJoCo 几何 ID 生成地面分割标签,无人工涂改。训练使用种子 0–5 共480张图,验证种子6共80张图,测试种子7–8共160张图。每个种子改变光照、墙面颜色及柜体位置,**属于同一走廊模板的变化,不是多个独立建筑**。
+
+样本通过静态相机位置/朝向采样获得。只在数据采集和回合重置时设置根节点位置,闭环测试中机器人由关节控制与接触动力学移动。以验证损失选择模型,训练20轮,Adam学习率0.001,批量16,随机种子42。MaskController 只保留最佳验证 MSE 和测试误差,没有保存逐轮曲线;perception、end2end 和 planner 的验证历史保存在 [evaluation.json](evaluation.json)。
+
+| 留出图像指标 | 结果 |
+|---|---:|
+| 地面分割 IoU | 0.7145 |
+| 端到端速度 MAE | 0.0287 m/s |
+| 端到端转向速度 MAE | 0.0781 rad/s |
+| 语义控制网络速度 MAE | 0.0117 m/s |
+| 语义控制网络转向速度 MAE | 0.0483 rad/s |
+
+种子9–12为追加的闭环测试,不参与训练或验证;它们仍然是同一走廊模板。训练图像误差不能代替闭环效果。下面是每个留出场景各一次物理运行,没有宣称统计显著性或大样本成功率。
+
+| 模式 | 场景种子 | 到达走廊测试终线 | 仿真耗时 | 环境碰撞步数 | 跌倒 |
+|---|---:|---|---:|---:|---|
+| 规则教师 | 7 | 是 | 30.36 s | 0 | 否 |
+| 感知控制 | 7 | 是 | 30.42 s | 0 | 否 |
+| 感知控制 | 8 | 是 | 32.10 s | 0 | 否 |
+| 端到端 | 7 | 是 | 29.46 s | 0 | 否 |
+| 端到端 | 8 | 是 | 31.58 s | 0 | 否 |
+| 感知控制 | 9 | 是 | 31.44 s | 0 | 否 |
+| 端到端 | 9 | 是 | 29.56 s | 0 | 否 |
+| 感知控制 | 10 | 是 | 30.42 s | 0 | 否 |
+| 端到端 | 10 | 是 | 30.00 s | 0 | 否 |
+| 感知控制 | 11 | 是 | 30.82 s | 0 | 否 |
+| 端到端 | 11 | 是 | 30.62 s | 0 | 否 |
+| 感知控制 | 12 | 是 | 30.70 s | 0 | 否 |
+| 端到端 | 12 | 是 | 31.14 s | 0 | 否 |
+
+终线定义为机器人根节点 x≥7.5m;这是测试终止条件,并非网络的输入或控制依据。碰撞只统计墙、门框、柜体接触;正常足地接触不计为障碍碰撞。原始试验摘要见 [evaluation.json](evaluation.json)。
+
+### 端到端近墙测试
+
+使用实际交付的 ImageController 权重,将骨盆初始位置分别设在距走廊端墙近表面 1.5、1.0、0.6 m 处,先零速稳定 2 秒,再运行控制 3 秒。三次均未跌倒,未记录与端墙的接触;最后一种情况下控制器最初输出零前进速度并转向。记录见 [near_obstacle_20261111.json](near_obstacle_20261111.json),复现使用源码目录的 `scripts/evaluate_near_obstacle.py --assets <资产路径> --output <结果路径>`。距离是初始骨盆到墙面的距离,不是机器人轮廓的间距;初始零速期间可能有漂移。仅测试单个静态墙面、朝向和种子,不能据此声称覆盖复杂障碍或保证安全。
+
+## 同步 SLAM 与导航
+
+180束平面雷达,5Hz,量程8m,排除机器人自体几何。ROS 桥发布 `/clock`、`/scan`、`/odom` 以及 `odom→base_link→laser`,gmapping 发布 `map→odom` 和 `/map`。图像消息的 `front_camera` 坐标系目前未发布对应 TF,因此不能直接用于跨坐标系融合。move_base 读取持续变化的地图,向 `/cmd_vel` 发布速度。
+
+测试顺序目标为 (2,-0.3)、(4,-0.3)、(7,0),三次均返回 action 状态3。位置容差 **0.5m**,终点朝向不约束。测试期间记录21次地图更新,已知栅格由11,143增至18,328。不能将该结果表述为厘米级导航。
+
+另一次神经辅助导航测试从实时地图和 TF 位置生成5个航点,最终目标为 (7,0),五个航点均返回 SUCCEEDED。CostNet 在240个随机障碍栅格上训练,验证与测试各40个独立种子,Dijkstra 距离为监督标签,训练20轮;测试距离 MAE 为0.7556格。
+
+CostNet 输入障碍图与相对目标的 x/y 坐标,使用空洞卷积预测剩余代价。A* 的主排序保持 `g + Manhattan`,仅在同一主代价时使用预测值打破平局,因此是神经辅助搜索。当前实测神经辅助与普通 A* 均扩展47个节点、路径代价均为46格,**没有观察到本回合的搜索加速**。未知栅格暂按可通行处理以支持探索,实际执行仍依赖实时雷达局部避障;不保证未知区域安全或任意地图都能到达。规划路径发布在 `/neural_patrol/path`,执行结果在 `/neural_patrol/result`。
+
+首次测试发现 TF 延迟与里程计角速度遗漏,修正后重新运行。步态零速仍有小幅漂移,长时间停车和精确停靠尚需进一步评价。
+
+## 安装与复现
+
+完整命令、环境、资产下载、四种模式和退出步骤见 [模块 README](https://github.com/OpenHUTB/ros2/tree/master/src/humanoid/humanoid_visual_patrol)。本页面待 PR 合并前,上述上游代码链接可能尚不存在,请使用本分支同路径。
+
+运行顺序:安装独立仿真环境 → 获取固定版本官方资产 → smoke → 遥控 → collect/train → perception/end2end → 启动 ROS 同步导航 → 保存地图 → 检查输出证据。
+
+四个模型权重随模块提供(分割约17KB,语义控制约61KB,图像控制约1.64MB,规划约40KB)。无需先训练即可测试,训练步骤可独立复现。大量数据和录像只保留本地;文档展示 GIF 小于5MB,PNG小于3MB。
+
+## 与已有模块的区别及限制
+
+已有 `humanoid_lidar_nn_perception` 以雷达扫描预测通行距离并用 MLP 避障。本模块学习 RGB 地面分割与图像速度回归,并集成同步 SLAM 多目标导航;它不是对原模块改名。共享的 G1 模型、官方步态和 ROS 软件明确归属于上游。
+
+目前缺少动态人群、复杂光照、不同建筑、大规模重复试验,以及跌倒后的恢复。视觉训练数据规模小,IoU仍有提升空间。外部机器复现和正式维护者信息尚待补齐,当前不能直接勾选全部 PR 验收项。
+
+## 大模型使用声明
+
+本项目使用 OpenAI Codex 辅助编程、调试、实验和文档整理。训练及仿真结果由实际程序产生;提交者需复核并对提交内容负责。官方模型、预训练策略及许可证详见源码 `third_party_notices.md`。
diff --git a/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json b/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json
new file mode 100644
index 0000000000..6cf05d96f8
--- /dev/null
+++ b/docs/humanoid/humanoid_visual_patrol/near_obstacle_20261111.json
@@ -0,0 +1,236 @@
+[
+ {
+ "distance_m": 1.5,
+ "settled": true,
+ "start_position": [
+ 8.417243170839143,
+ -0.01578553768493515,
+ 0.775756219577456
+ ],
+ "final_position": [
+ 8.730576156913775,
+ -0.16362254449528404,
+ 0.7793329966064625
+ ],
+ "fallen": false,
+ "wall_contact_steps": 0,
+ "commands": [
+ [
+ 0.14146247506141663,
+ -0.17015895247459412
+ ],
+ [
+ 0.11891498416662216,
+ -0.24590933322906494
+ ],
+ [
+ 0.1121121346950531,
+ -0.22971871495246887
+ ],
+ [
+ 0.11599573493003845,
+ -0.21102428436279297
+ ],
+ [
+ 0.13836979866027832,
+ -0.2381228506565094
+ ],
+ [
+ 0.1183127835392952,
+ -0.33010783791542053
+ ],
+ [
+ 0.10987158119678497,
+ -0.3273693919181824
+ ],
+ [
+ 0.12709695100784302,
+ -0.2385319620370865
+ ],
+ [
+ 0.13229939341545105,
+ -0.25760507583618164
+ ],
+ [
+ 0.10650484263896942,
+ -0.35021722316741943
+ ],
+ [
+ 0.11009601503610611,
+ -0.26059627532958984
+ ],
+ [
+ 0.1392822563648224,
+ -0.13386747241020203
+ ],
+ [
+ 0.1288258582353592,
+ -0.20020955801010132
+ ],
+ [
+ 0.10298807919025421,
+ -0.2853042483329773
+ ],
+ [
+ 0.12290354818105698,
+ -0.17758265137672424
+ ]
+ ]
+ },
+ {
+ "distance_m": 1.0,
+ "settled": true,
+ "start_position": [
+ 8.917243170839134,
+ -0.015785537684923127,
+ 0.7757562195774552
+ ],
+ "final_position": [
+ 9.090908049428544,
+ -0.10127337209597133,
+ 0.781319078402443
+ ],
+ "fallen": false,
+ "wall_contact_steps": 0,
+ "commands": [
+ [
+ 0.08929011225700378,
+ -0.09286505728960037
+ ],
+ [
+ 0.08064748346805573,
+ -0.1253059059381485
+ ],
+ [
+ 0.07548756152391434,
+ -0.11916515976190567
+ ],
+ [
+ 0.07235558331012726,
+ -0.1257079541683197
+ ],
+ [
+ 0.06656993180513382,
+ -0.21493399143218994
+ ],
+ [
+ 0.03834439069032669,
+ -0.3157784938812256
+ ],
+ [
+ 0.025117628276348114,
+ -0.3475883901119232
+ ],
+ [
+ 0.02754291146993637,
+ -0.3555821180343628
+ ],
+ [
+ 0.01775287836790085,
+ -0.4294828176498413
+ ],
+ [
+ 0.013274461030960083,
+ -0.45
+ ],
+ [
+ 0.029487960040569305,
+ -0.42537784576416016
+ ],
+ [
+ 0.058489810675382614,
+ -0.33308491110801697
+ ],
+ [
+ 0.06443822383880615,
+ -0.3198435604572296
+ ],
+ [
+ 0.05350431799888611,
+ -0.36891812086105347
+ ],
+ [
+ 0.07223033905029297,
+ -0.2864035665988922
+ ]
+ ]
+ },
+ {
+ "distance_m": 0.6,
+ "settled": true,
+ "start_position": [
+ 9.317243170839143,
+ -0.0157855376849174,
+ 0.7757562195774554
+ ],
+ "final_position": [
+ 9.386478171746983,
+ -0.07180124879056617,
+ 0.7820914707217922
+ ],
+ "fallen": false,
+ "wall_contact_steps": 0,
+ "commands": [
+ [
+ 0.0,
+ -0.07231006026268005
+ ],
+ [
+ 0.0,
+ -0.13426412642002106
+ ],
+ [
+ 0.0,
+ -0.12162493169307709
+ ],
+ [
+ 0.0,
+ -0.1387452930212021
+ ],
+ [
+ 0.0,
+ -0.2208845317363739
+ ],
+ [
+ 0.0,
+ -0.33562785387039185
+ ],
+ [
+ 0.0,
+ -0.35593006014823914
+ ],
+ [
+ 0.0,
+ -0.39087188243865967
+ ],
+ [
+ 0.0,
+ -0.45
+ ],
+ [
+ 0.0,
+ -0.45
+ ],
+ [
+ 0.0,
+ -0.45
+ ],
+ [
+ 0.0,
+ -0.45
+ ],
+ [
+ 0.0,
+ -0.41778644919395447
+ ],
+ [
+ 0.041777290403842926,
+ -0.35124245285987854
+ ],
+ [
+ 0.07274018228054047,
+ -0.26592519879341125
+ ]
+ ]
+ }
+]
diff --git a/docs/img/humanoid_visual_patrol/end2end_7.gif b/docs/img/humanoid_visual_patrol/end2end_7.gif
new file mode 100644
index 0000000000..de0550a204
Binary files /dev/null and b/docs/img/humanoid_visual_patrol/end2end_7.gif differ
diff --git a/docs/img/humanoid_visual_patrol/end2end_7.png b/docs/img/humanoid_visual_patrol/end2end_7.png
new file mode 100644
index 0000000000..b2b5756e7a
Binary files /dev/null and b/docs/img/humanoid_visual_patrol/end2end_7.png differ
diff --git a/docs/img/humanoid_visual_patrol/perception_7.gif b/docs/img/humanoid_visual_patrol/perception_7.gif
new file mode 100644
index 0000000000..421110ed17
Binary files /dev/null and b/docs/img/humanoid_visual_patrol/perception_7.gif differ
diff --git a/docs/img/humanoid_visual_patrol/slam_map.png b/docs/img/humanoid_visual_patrol/slam_map.png
new file mode 100644
index 0000000000..e88d3ac6ea
Binary files /dev/null and b/docs/img/humanoid_visual_patrol/slam_map.png differ
diff --git a/docs/index.md b/docs/index.md
index af2b8fd076..9e3a1dd814 100644
--- a/docs/index.md
+++ b/docs/index.md
@@ -6,6 +6,8 @@ title: 主页
文档环境初始化:[Ubuntu 一键安装与本地预览](./ubuntu_docs_setup.md)。
+人形机器人示例:[视觉通行感知与室内巡检](./humanoid/humanoid_visual_patrol/index.md),包含 MuJoCo G1、RGB 分割控制、同步 SLAM 导航和图像速度策略。
+
**提交项目之前**,请按以下指引自查,并阅读[仓库提交约定](https://github.com/OpenHUTB/ros2#注意事项)。PR 模板只列常见检查项;具体任务、模拟器和验收要求以仓库约定及维护者说明为准。
diff --git a/mkdocs.yml b/mkdocs.yml
index 58a39225e1..d787bec5d7 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -75,6 +75,7 @@ nav:
- 10. Turtlebot3 Gazebo键盘控制: 'chapter/turtlebot3_gazebo_control.md'
- 人形机器人:
+ - 人形机器人视觉通行感知与室内巡检: './humanoid/humanoid_visual_patrol/index.md'
- 人形机器人基础: './humanoid/humanoid_basics.md'
- 双臂搬运负载的神经网络平衡控制: './humanoid/humanoid_carry_payload_nn.md'
- 双足步态生成与 ZMP 稳定性: './humanoid/bipedal_gait_zmp.md'
diff --git a/src/humanoid/humanoid_visual_patrol/.gitattributes b/src/humanoid/humanoid_visual_patrol/.gitattributes
new file mode 100644
index 0000000000..61d299b53f
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/.gitattributes
@@ -0,0 +1,2 @@
+*.py text eol=lf
+*.sh text eol=lf
diff --git a/src/humanoid/humanoid_visual_patrol/CMakeLists.txt b/src/humanoid/humanoid_visual_patrol/CMakeLists.txt
new file mode 100644
index 0000000000..d23806fcda
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/CMakeLists.txt
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(humanoid_visual_patrol)
+find_package(catkin REQUIRED)
+catkin_package()
+catkin_install_python(PROGRAMS scripts/ros_bridge.py scripts/task_runner.py scripts/neural_planner.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/src/humanoid/humanoid_visual_patrol/README.md b/src/humanoid/humanoid_visual_patrol/README.md
new file mode 100644
index 0000000000..946fc81218
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/README.md
@@ -0,0 +1,104 @@
+# 人形机器人视觉通行感知与室内导航
+
+在 MuJoCo 3.2.0 中使用 Unitree G1 的预训练步态,开发 RGB 地面分割、图像到速度控制及 ROS Noetic 同步 SLAM 导航。模块名:`humanoid_visual_patrol`。
+
+完整原理、实验和限制见 [课程文档](../../../docs/humanoid/humanoid_visual_patrol/index.md)。
+
+## 功能与边界
+
+| 任务 | 入口 | 实现 |
+|---|---|---|
+| 键盘控制 | `main.py --mode teleop` | WASD/方向键、空格停止、加减速度,0.6秒未按键归零 |
+| 感知控制 | `--mode perception` | RGB → FloorNet 地面分割 → MaskController 语义控制网络 → 行走速度 |
+| 建图导航 | `launch/main.launch` 默认 navigation | 模拟雷达及积分里程计 → gmapping → CostNet 辅助 A* → move_base → G1 |
+| 端到端 | `--mode end2end` | RGB → ImageController → 前进/转向速度 → 预训练步态 |
+
+任务二的分割与语义控制网络分别训练,规则通道选择只用于生成示范及对照;部署时若分割图完全没有地面像素,额外的规则保护会将速度和转向置零,并不代表网络自身能处理这种情况。该保护不覆盖少量误分割或所有障碍。任务三使用经典 gmapping、CostNet 辅助全局 A* 和 move_base 局部避障,是混合规划方案,并非神经网络 SLAM。任务四直接输出高层速度,不是图像到关节力矩。底层步态是第三方预训练策略,不计作本项目训练成果。
+
+本示例的终线是通行测试终点,场景标记不触发巡检事件。
+
+## 已验证环境
+
+- 当前复现步骤仅验证源码空间运行,不保证 catkin install 空间运行。
+- `catkin_make` 源码空间构建通过;`catkin_make install` 构建通过,但安装空间只有 scripts/launch/config,不含 `models/` 与 `patrol` 包,因此仍按源码空间运行。
+- Windows 11,Python 3.11.15,MuJoCo 3.2.0,PyTorch 2.15.0+cpu:物理仿真、图像采集、训练、离屏录像。
+- WSL2 Ubuntu 20.04.5,ROS Noetic,Python 3.8.10:gmapping、move_base 和 ROS 桥。
+- Windows 和 WSL 用子进程标准输入输出连接,不需要开放网络端口给仿真器。
+- 本次使用 CPU 训练;未声明 GPU 性能。原生 Linux 仿真解释器路径受支持,但本次未验证该部署组合。
+
+## 安装与获取资产
+
+在本模块目录运行(以下 `python` 指独立仿真环境解释器):
+
+```bash
+python -m pip install -r requirements.txt
+python scripts/fetch_assets.py /path/to/unitree_rl_gym
+```
+
+资产固定到官方 `unitree_rl_gym` 提交 `276801e46c5d433564f24658bac64f254b7d2d4b`,含 G1 12 关节腿部模型、网格和 `deploy/pre_train/g1/motion.pt`。上半身模型固定,不能声称实现全身灵巧操作。许可证见 `third_party_notices.md`。
+
+```bash
+python main.py --assets /path/to/unitree_rl_gym --mode smoke --output results/smoke.json
+python main.py --assets /path/to/unitree_rl_gym --mode teleop
+python main.py --assets /path/to/unitree_rl_gym --mode perception --weights models --record --output results
+python main.py --assets /path/to/unitree_rl_gym --mode end2end --weights models --record --output results
+```
+
+关闭可视窗口停止遥控,默认45仿真秒后退出,可用 `--seconds` 调整。需连续按键维持运动;已通过向真实 GLFW 窗口发送 Windows 按键事件验证前进、转向和停止,仍建议审核者实际操作复核手感。
+
+## 独立训练与测试
+
+```bash
+python main.py --assets /path/to/unitree_rl_gym --mode collect --dataset data/visual_patrol --samples 80
+python main.py --assets /path/to/unitree_rl_gym --mode train --dataset data/visual_patrol --weights models --epochs 20
+python main.py --assets /path/to/unitree_rl_gym --mode train_planner --weights models
+python main.py --assets /path/to/unitree_rl_gym --mode perception --weights models --seed 8 --output results --record
+python main.py --assets /path/to/unitree_rl_gym --mode end2end --weights models --seed 8 --output results --record
+```
+
+训练/验证/测试分别使用场景种子 0–5、6、7–8,每个场景 80 张图像。随机化涉及光照、颜色和柜体侧向位置;两套测试场景并不代表不同建筑。标签由仿真几何 ID 产生,训练图像来自静态相机姿态采样,闭环测试中才使用真实步态运动。测试结果不能外推到楼梯、人群或真实机器人。
+
+## ROS 启动(本次验证方式)
+
+在 WSL 中,以本模块目录为当前目录:
+
+```bash
+bash scripts/run_navigation.sh \
+ /mnt/f/path/to/.venv_humanoid/Scripts/python.exe \
+ /mnt/f/path/to/unitree_rl_gym navigation
+```
+
+脚本自动转换 Windows 子进程使用的路径。`ROS_MASTER_URI` 默认为 `http://localhost:11331`;其他终端也需设置这个值。手动设置路径时:
+
+```bash
+source /opt/ros/noetic/setup.bash
+export ROS_MASTER_URI=http://localhost:11331
+export ROS_PACKAGE_PATH="$PWD:$ROS_PACKAGE_PATH"
+roslaunch humanoid_visual_patrol main.launch \
+ sim_python:=/path/to/python sim_entry:=/path/to/main.py assets:=/path/to/unitree_rl_gym
+```
+
+先等待 `/map` 更新,再通过 RViz 的 2D Nav Goal 发送地图坐标目标,由神经辅助规划节点生成并执行航点;直接调用 `/move_base` action 则用于经典规划基线对照。导航与 SLAM 同时运行;位置容差 0.5m,终点朝向不约束。不要同时运行独立遥控和导航去控制同一个仿真实例。
+
+`task:=teleop`、`task:=perception`、`task:=end2end` 可以经相同 launch 调用对应任务。后两项需传入 `weights`,输出路径通过 `output` 指定。
+
+```bash
+rosrun map_server map_saver -f /path/to/saved_map
+```
+
+Ctrl+C 退出 launch 后桥接进程会关闭其仿真子进程。
+
+## 本地验证
+
+```bash
+python -m pip install -r requirements-dev.txt
+# Linux: export HUMANOID_ASSETS=/path/to/unitree_rl_gym
+# PowerShell: $env:HUMANOID_ASSETS='F:/path/to/unitree_rl_gym'
+python -m pytest tests -q
+```
+
+包括无地面时停止、左右通行方向、神经平局排序不改变最优栅格路径代价、无路径报错,以及真实动力学行走和雷达自体回波排除。没有资产路径时,物理测试明确跳过。
+
+## 大模型使用声明
+
+本模块使用 OpenAI Codex 辅助方案、编程、调试、文档和实验整理。发布者需对内容复核并负责;预训练模型和第三方来源已单独说明。
diff --git a/src/humanoid/humanoid_visual_patrol/config/navigation.yaml b/src/humanoid/humanoid_visual_patrol/config/navigation.yaml
new file mode 100644
index 0000000000..d4b73c4910
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/config/navigation.yaml
@@ -0,0 +1,57 @@
+controller_frequency: 5.0
+planner_frequency: 1.0
+controller_patience: 15.0
+planner_patience: 10.0
+recovery_behavior_enabled: false
+base_local_planner: base_local_planner/TrajectoryPlannerROS
+TrajectoryPlannerROS:
+ max_vel_x: 0.3
+ min_vel_x: 0.05
+ max_vel_theta: 0.35
+ min_vel_theta: -0.35
+ min_in_place_vel_theta: 0.15
+ acc_lim_x: 0.3
+ acc_lim_y: 0.0
+ acc_lim_theta: 0.4
+ holonomic_robot: false
+ # Patrol goals constrain position; no final heading is prescribed.
+ yaw_goal_tolerance: 3.14159
+ xy_goal_tolerance: 0.5
+ latch_xy_goal_tolerance: true
+ sim_time: 2.0
+ vx_samples: 8
+ vtheta_samples: 16
+ meter_scoring: true
+ pdist_scale: 0.8
+ gdist_scale: 0.6
+ occdist_scale: 0.05
+global_costmap:
+ global_frame: map
+ robot_base_frame: base_link
+ update_frequency: 3.0
+ publish_frequency: 1.0
+ transform_tolerance: 1.0
+ robot_radius: 0.3
+ plugins:
+ - {name: static_layer, type: 'costmap_2d::StaticLayer'}
+ - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
+ static_layer: {subscribe_to_updates: true}
+ inflation_layer: {inflation_radius: 0.6, cost_scaling_factor: 3.0}
+local_costmap:
+ global_frame: odom
+ robot_base_frame: base_link
+ rolling_window: true
+ width: 5.0
+ height: 5.0
+ resolution: 0.05
+ update_frequency: 5.0
+ publish_frequency: 1.0
+ transform_tolerance: 1.0
+ robot_radius: 0.3
+ plugins:
+ - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
+ - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
+ obstacle_layer:
+ observation_sources: laser
+ laser: {sensor_frame: laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true, obstacle_range: 7.5, raytrace_range: 8.0}
+ inflation_layer: {inflation_radius: 0.6, cost_scaling_factor: 3.0}
diff --git a/src/humanoid/humanoid_visual_patrol/launch/main.launch b/src/humanoid/humanoid_visual_patrol/launch/main.launch
new file mode 100644
index 0000000000..1c45d50d5e
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/launch/main.launch
@@ -0,0 +1,51 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/humanoid/humanoid_visual_patrol/main.py b/src/humanoid/humanoid_visual_patrol/main.py
new file mode 100644
index 0000000000..2827fe4a73
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/main.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+"""Train and run humanoid visual patrol, teleoperation and ROS bridge tasks."""
+import argparse
+import json
+from pathlib import Path
+
+
+def main():
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('--assets', required=True, help='Path to unitree_rl_gym checkout')
+ parser.add_argument('--mode', choices=['smoke', 'collect', 'train', 'teacher', 'perception', 'end2end', 'teleop', 'bridge', 'train_planner', 'plan_stdio'], default='smoke')
+ parser.add_argument('--output', type=Path)
+ parser.add_argument('--dataset', type=Path, default=Path('data/visual_patrol'))
+ parser.add_argument('--weights', type=Path, default=Path(__file__).resolve().parent / 'models')
+ parser.add_argument('--samples', type=int, default=100)
+ parser.add_argument('--epochs', type=int, default=15)
+ parser.add_argument('--seed', type=int, default=7)
+ parser.add_argument('--seconds', type=float, default=45)
+ parser.add_argument('--record', action='store_true')
+ parser.add_argument('--controller',choices=['navigation','perception','end2end'],default='navigation')
+ args = parser.parse_args()
+ if args.mode == 'train_planner':
+ from patrol.planning import train_planner
+ print(json.dumps(train_planner(args.weights),indent=2))
+ return
+ if args.mode == 'plan_stdio':
+ import sys
+ from patrol.planning import plan_request
+ print(json.dumps(plan_request(json.load(sys.stdin),args.weights)))
+ return
+ if args.mode == 'bridge':
+ from patrol.bridge_server import serve
+ serve(args.assets,args.seed,args.controller,args.weights)
+ return
+ if args.mode == 'collect':
+ from patrol.learning import collect
+ collect(args.assets, args.dataset, args.samples)
+ return
+ if args.mode == 'train':
+ from patrol.learning import train, train_mask_controller
+ train(args.dataset, args.weights, args.epochs)
+ train_mask_controller(args.dataset,args.weights)
+ return
+ if args.mode == 'teleop':
+ from patrol.runtime import teleop
+ teleop(args.assets,args.seed,args.seconds,args.output)
+ return
+ if args.mode in ('teacher','perception','end2end'):
+ from patrol.runtime import rollout
+ result = rollout(args.assets,args.mode,args.weights,args.output or Path('results'),
+ args.seed,args.seconds,args.record)
+ print(json.dumps(result,indent=2))
+ return
+ from patrol.locomotion import Walker
+ result = Walker(args.assets).smoke_test()
+ report = json.dumps(result, indent=2)
+ print(report)
+ if args.output:
+ args.output.parent.mkdir(parents=True, exist_ok=True)
+ args.output.write_text(report+'\n', encoding='utf-8')
+ if result['fallen'] or result['displacement_m'] < 0.5:
+ raise SystemExit(1)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/humanoid_visual_patrol/models/end2end.pt b/src/humanoid/humanoid_visual_patrol/models/end2end.pt
new file mode 100644
index 0000000000..871eac4fb1
Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/end2end.pt differ
diff --git a/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt b/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt
new file mode 100644
index 0000000000..5bcc069e20
Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/mask_controller.pt differ
diff --git a/src/humanoid/humanoid_visual_patrol/models/perception.pt b/src/humanoid/humanoid_visual_patrol/models/perception.pt
new file mode 100644
index 0000000000..1c8b1565dd
Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/perception.pt differ
diff --git a/src/humanoid/humanoid_visual_patrol/models/planner.pt b/src/humanoid/humanoid_visual_patrol/models/planner.pt
new file mode 100644
index 0000000000..97c8a1cad3
Binary files /dev/null and b/src/humanoid/humanoid_visual_patrol/models/planner.pt differ
diff --git a/src/humanoid/humanoid_visual_patrol/package.xml b/src/humanoid/humanoid_visual_patrol/package.xml
new file mode 100644
index 0000000000..53c60223be
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/package.xml
@@ -0,0 +1,13 @@
+
+
+ humanoid_visual_patrol0.1.0
+ MuJoCo G1 visual floor perception, image control and ROS indoor navigation.
+ OpenHUTB contributor
+ BSD-3-Clause
+ catkin
+ rospytf
+ sensor_msgsgeometry_msgs
+ nav_msgsrosgraph_msgs
+ gmappingmove_base
+ actionlibmove_base_msgsstd_msgs
+
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/__init__.py b/src/humanoid/humanoid_visual_patrol/patrol/__init__.py
new file mode 100644
index 0000000000..bc4204b4fb
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/__init__.py
@@ -0,0 +1 @@
+"""Visual patrol on a physics simulated G1 humanoid."""
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py b/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py
new file mode 100644
index 0000000000..c358ab2ac3
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/bridge_server.py
@@ -0,0 +1,43 @@
+"""JSON lines over stdio: a process boundary between ROS and simulator Python."""
+import json
+import sys
+import base64
+from pathlib import Path
+
+from .scene import IndoorSimulator
+
+
+def serve(assets, seed=0, controller='navigation',weights=None):
+ sim = IndoorSimulator(assets,seed)
+ if controller in ('perception','end2end'):
+ from .vision import VisionPolicy
+ policy=VisionPolicy(Path(weights)/f'{controller}.pt',controller)
+ else:
+ policy=None
+ try:
+ for line in sys.stdin:
+ request = json.loads(line)
+ if request.get('stop'):
+ break
+ command = request.get('command',[0,0,0])
+ if policy and sim.data.time>=2:
+ prediction,_=policy.command(sim.image())
+ command=[float(prediction[0]),0,float(prediction[1])]
+ sim.set_command(*command)
+ previous_yaw = sim.odom[2]
+ previous_time = sim.data.time
+ for _ in range(10):
+ sim.step()
+ if sim.fallen:
+ break
+ rgb=sim.image()
+ result = dict(time=float(sim.data.time), odom=sim.odom.tolist(),
+ scan=sim.scan(), velocity=[float(sim.data.qvel[0]),float(sim.data.qvel[1])],
+ angular_velocity=float((sim.odom[2]-previous_yaw)/(sim.data.time-previous_time)),
+ fallen=bool(sim.fallen), joints=sim.data.qpos[7:].tolist(),
+ joint_names=[sim.model.joint(i).name for i in range(1,sim.model.njnt)],
+ image=base64.b64encode(rgb.tobytes()).decode('ascii'),
+ image_shape=list(rgb.shape))
+ print(json.dumps(result),flush=True)
+ finally:
+ sim.close()
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/learning.py b/src/humanoid/humanoid_visual_patrol/patrol/learning.py
new file mode 100644
index 0000000000..0c1d12a45b
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/learning.py
@@ -0,0 +1,134 @@
+"""Reproducible simulator data and held-out-scene evaluation."""
+import json
+from pathlib import Path
+import time
+
+import mujoco
+import numpy as np
+import torch
+from torch import nn
+from torch.utils.data import DataLoader, TensorDataset
+
+from .scene import IndoorSimulator
+from .vision import FloorNet, ImageController, MaskController, mask_command
+
+
+def collect(assets, destination, samples=100):
+ destination = Path(destination)
+ destination.mkdir(parents=True, exist_ok=True)
+ for split, seeds in [('train', range(6)), ('validation', [6]), ('test', [7, 8])]:
+ images, masks, actions, scene_ids = [], [], [], []
+ for seed in seeds:
+ sim = IndoorSimulator(assets, seed)
+ rng = np.random.default_rng(1000+seed)
+ try:
+ for _ in range(samples):
+ # Independent static camera sampling, NOT a runtime walking method.
+ mujoco.mj_resetData(sim.model, sim.data)
+ sim.data.qpos[:2] = [rng.uniform(.0, 8.8), rng.uniform(-.9, .9)]
+ yaw = rng.uniform(-.65, .65)
+ sim.data.qpos[3:7] = [np.cos(yaw/2), 0, 0, np.sin(yaw/2)]
+ sim.data.qpos[7:] = sim.default
+ mujoco.mj_forward(sim.model, sim.data)
+ rgb, mask = sim.image(labels=True)
+ images.append(rgb)
+ masks.append(mask)
+ actions.append(mask_command(mask))
+ scene_ids.append(seed)
+ finally:
+ sim.close()
+ print(f'collected {split} scene={seed}', flush=True)
+ np.savez_compressed(destination/f'{split}.npz', images=np.asarray(images),
+ masks=np.asarray(masks), actions=np.asarray(actions), scene_ids=scene_ids)
+ metadata = dict(split_by='scene_seed', train=list(range(6)), validation=[6], test=[7,8],
+ samples_per_scene=samples, labels='MuJoCo geometry-ID floor masks',
+ teacher='reactive floor-mask corridor following',
+ limitations='Static camera poses; appearance/layout randomization is limited. Closed-loop testing required.')
+ (destination/'dataset.json').write_text(json.dumps(metadata, indent=2), encoding='utf-8')
+
+
+def train(dataset, output, epochs=15):
+ torch.manual_seed(42)
+ torch.set_num_threads(4)
+ dataset, output = Path(dataset), Path(output)
+ output.mkdir(parents=True, exist_ok=True)
+ def read(split):
+ with np.load(dataset/f'{split}.npz') as d:
+ return (torch.from_numpy(d['images']).permute(0,3,1,2).float()/255,
+ torch.from_numpy(d['masks']).float()[:,None],
+ torch.from_numpy(d['actions']).float())
+ train_data, val_data, test_data = read('train'), read('validation'), read('test')
+ metrics = {}
+ for mode, model, idx in [('perception', FloorNet(), 1), ('end2end', ImageController(), 2)]:
+ loader = DataLoader(TensorDataset(train_data[0],train_data[idx]), batch_size=16, shuffle=True)
+ optimizer = torch.optim.Adam(model.parameters(), lr=.001)
+ loss_fn = nn.BCEWithLogitsLoss() if idx == 1 else nn.MSELoss()
+ best = float('inf')
+ history = []
+ start = time.time()
+ for epoch in range(epochs):
+ model.train()
+ total = 0.
+ for x,y in loader:
+ optimizer.zero_grad()
+ loss = loss_fn(model(x),y)
+ loss.backward()
+ optimizer.step()
+ total += float(loss.detach())*len(x)
+ model.eval()
+ with torch.inference_mode():
+ val = float(loss_fn(model(val_data[0]),val_data[idx]))
+ history.append(dict(epoch=epoch+1, train_loss=total/len(train_data[0]), validation_loss=val))
+ if val < best:
+ best = val
+ torch.save(model.state_dict(),output/f'{mode}.pt')
+ print(mode,history[-1],flush=True)
+ model.load_state_dict(torch.load(output/f'{mode}.pt',weights_only=True))
+ with torch.inference_mode():
+ pred = model(test_data[0])
+ if idx == 1:
+ pred = pred.sigmoid()>.5
+ truth = test_data[idx]>.5
+ score = dict(floor_iou=float((pred & truth).sum()/(pred | truth).sum().clamp(min=1)))
+ else:
+ error = (pred-test_data[idx]).abs().mean(0)
+ score = dict(speed_mae_mps=float(error[0]), yaw_rate_mae_radps=float(error[1]))
+ metrics[mode] = dict(test=score, history=history, training_seconds=time.time()-start)
+ (output/'metrics.json').write_text(json.dumps(metrics,indent=2),encoding='utf-8')
+ return metrics
+
+
+def train_mask_controller(dataset, output, epochs=100):
+ """Train on scene-disjoint semantic masks; test only after model selection."""
+ torch.manual_seed(73)
+ torch.set_num_threads(2)
+ dataset,output=Path(dataset),Path(output)
+ def load(split):
+ with np.load(dataset/f'{split}.npz') as d:
+ return torch.from_numpy(d['masks']).float()[:,None],torch.from_numpy(d['actions'])
+ x,y=load('train')
+ xv,yv=load('validation')
+ xt,yt=load('test')
+ model=MaskController()
+ optimizer=torch.optim.Adam(model.parameters(),lr=.002)
+ loader=DataLoader(TensorDataset(x,y),batch_size=32,shuffle=True)
+ best=float('inf')
+ for epoch in range(epochs):
+ model.train()
+ for xb,yb in loader:
+ loss=nn.functional.mse_loss(model(xb),yb)
+ optimizer.zero_grad(); loss.backward(); optimizer.step()
+ model.eval()
+ with torch.inference_mode():
+ val=float(nn.functional.mse_loss(model(xv),yv))
+ if val= 10 else 0.3,
+ wz=0.25 if 6 <= t < 10 else 0.0)
+ self.step()
+ min_height = min(min_height, float(self.data.qpos[2]))
+ if self.fallen:
+ break
+ return dict(simulated_seconds=float(self.data.time), fallen=bool(self.fallen),
+ displacement_m=float(np.linalg.norm(self.data.qpos[:2]-start[:2])),
+ min_height_m=min_height, final_yaw_rad=float(self.yaw),
+ final_position=self.data.qpos[:3].tolist())
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/planning.py b/src/humanoid/humanoid_visual_patrol/patrol/planning.py
new file mode 100644
index 0000000000..ee38a57ac0
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/planning.py
@@ -0,0 +1,159 @@
+"""Learned cost-to-go tie breaker for admissible grid A*.
+
+Manhattan f is the primary priority, so learned estimates cannot invalidate
+optimality on the four-connected grid. The network resolves equal-f plateaus.
+This is neural-assisted search, not an end-to-end replacement of graph search.
+"""
+import heapq
+import json
+from pathlib import Path
+
+import numpy as np
+import torch
+from torch import nn
+
+
+class CostNet(nn.Module):
+ def __init__(self):
+ super().__init__()
+ self.net=nn.Sequential(nn.Conv2d(3,16,3,padding=1),nn.ReLU(),
+ nn.Conv2d(16,24,3,padding=2,dilation=2),nn.ReLU(),
+ nn.Conv2d(24,24,3,padding=4,dilation=4),nn.ReLU(),
+ nn.Conv2d(24,1,1),nn.Softplus())
+
+ def forward(self,x):
+ return self.net(x)
+
+
+def features(grid,goal):
+ yy,xx=np.indices(grid.shape)
+ return np.stack([grid.astype(np.float32),(xx-goal[1])/32.,(yy-goal[0])/32.]).astype(np.float32)
+
+
+def distances(grid,goal):
+ result=np.full(grid.shape,np.inf,dtype=np.float32)
+ result[goal]=0
+ queue=[(0,goal)]
+ while queue:
+ cost,point=heapq.heappop(queue)
+ if cost!=result[point]: continue
+ for dy,dx in [(1,0),(-1,0),(0,1),(0,-1)]:
+ q=(point[0]+dy,point[1]+dx)
+ if 0<=q[0]50
+ radius=int(np.ceil(.3/resolution))
+ inflated=blocked.copy()
+ for dy in range(-radius,radius+1):
+ for dx in range(-radius,radius+1):
+ if dx*dx+dy*dy>radius*radius: continue
+ srcy=slice(max(0,-dy),min(len(blocked),len(blocked)-dy))
+ srcx=slice(max(0,-dx),min(blocked.shape[1],blocked.shape[1]-dx))
+ dsty=slice(max(0,dy),min(len(blocked),len(blocked)+dy))
+ dstx=slice(max(0,dx),min(blocked.shape[1],blocked.shape[1]+dx))
+ inflated[dsty,dstx]|=blocked[srcy,srcx]
+ stride=3
+ hh,ww=inflated.shape[0]//stride,inflated.shape[1]//stride
+ grid=inflated[:hh*stride,:ww*stride].reshape(hh,stride,ww,stride).any(axis=(1,3))
+ cell=resolution*stride;ox,oy=request['origin']
+ def index(xy):
+ p=(int((xy[1]-oy)/cell),int((xy[0]-ox)/cell))
+ if not(0<=p[0]=1.5:
+ keep.append(route[i])
+ keep.append(route[-1])
+ world=[[ox+(p[1]+.5)*cell,oy+(p[0]+.5)*cell] for p in keep[1:]]
+ world[-1]=request['goal']
+ return dict(waypoints=world,expanded=expanded,baseline_expanded=baseline_expanded,
+ path_cost=len(route)-1,baseline_path_cost=len(baseline)-1,
+ unknown_policy='provisionally traversable; live local obstacle avoidance required',
+ method='Manhattan A* with neural equal-f tie breaking')
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/runtime.py b/src/humanoid/humanoid_visual_patrol/patrol/runtime.py
new file mode 100644
index 0000000000..7f381f483b
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/runtime.py
@@ -0,0 +1,110 @@
+"""Closed-loop rollouts, keyboard control, and actual simulator recordings."""
+import json
+from pathlib import Path
+import sys
+import time
+
+import imageio.v2 as imageio
+import mujoco
+import numpy as np
+from PIL import Image, ImageDraw
+
+from .scene import IndoorSimulator
+from .vision import VisionPolicy, mask_command
+
+
+def rollout(assets, mode, weights, output, seed=7, seconds=45, record=False):
+ output = Path(output)
+ output.mkdir(parents=True, exist_ok=True)
+ sim = IndoorSimulator(assets,seed)
+ policy = VisionPolicy(Path(weights)/f'{mode}.pt', mode) if mode in ('perception','end2end') else None
+ trajectory, frames = [], []
+ overview = mujoco.Renderer(sim.model,height=240,width=320) if record else None
+ camera = mujoco.MjvCamera()
+ camera.azimuth, camera.elevation, camera.distance = 140, -65, 3.5
+ collision_steps = 0
+ previous = np.zeros(2)
+ start = time.time()
+ try:
+ for tick in range(int(seconds/.02)):
+ if tick%10 == 0:
+ rgb, mask = sim.image(labels=True)
+ command = policy.command(rgb)[0] if policy else mask_command(mask)
+ if tick<100:
+ command[:] = 0
+ previous = .6*previous+.4*command
+ sim.set_command(float(previous[0]),wz=float(previous[1]))
+ trajectory.append([float(sim.data.time), *sim.data.qpos[:3].tolist(), sim.yaw,
+ *previous.tolist(), *sim.odom.tolist()])
+ if overview and tick%20 == 0:
+ camera.lookat[:] = sim.data.qpos[:3]
+ overview.update_scene(sim.data,camera=camera)
+ frame = Image.fromarray(overview.render())
+ inset = Image.fromarray(rgb).resize((128,96))
+ frame.paste(inset,(192,0))
+ ImageDraw.Draw(frame).text((8,8),f'{mode} t={sim.data.time:.1f}s',fill='white')
+ frames.append(np.asarray(frame))
+ sim.step()
+ for contact in sim.data.contact:
+ names = [mujoco.mj_id2name(sim.model,mujoco.mjtObj.mjOBJ_GEOM,int(g)) or ''
+ for g in (contact.geom1,contact.geom2)]
+ if any(n.endswith('wall') or n.startswith('door_') or n=='cabinet' for n in names):
+ collision_steps += 1
+ break
+ if sim.fallen or sim.data.qpos[0]>=7.5:
+ break
+ result = dict(mode=mode,scene_seed=seed,simulated_seconds=float(sim.data.time),
+ wall_seconds=time.time()-start,fallen=bool(sim.fallen),
+ reached=bool(sim.data.qpos[0]>=7.5 and not sim.fallen),
+ final_position=sim.data.qpos[:3].tolist(),collision_steps=collision_steps,
+ odometry_error_m=float(np.linalg.norm(sim.odom[:2]-sim.data.qpos[:2])))
+ (output/f'{mode}_{seed}.json').write_text(json.dumps(result,indent=2),encoding='utf-8')
+ np.savetxt(output/f'{mode}_{seed}.csv',trajectory,delimiter=',',
+ header='time,x,y,z,yaw,vx,wz,odom_x,odom_y,odom_yaw',comments='')
+ if frames:
+ imageio.mimsave(output/f'{mode}_{seed}.gif',frames,duration=.4,loop=0)
+ Image.fromarray(frames[-1]).save(output/f'{mode}_{seed}.png')
+ return result
+ finally:
+ if overview:
+ overview.close()
+ sim.close()
+
+
+def teleop(assets,seed=0,seconds=45,output=None):
+ """Interactive MuJoCo viewer; arrow/WASD commands expire after 0.6s."""
+ import mujoco.viewer
+ sim = IndoorSimulator(assets,seed)
+ command = np.zeros(2)
+ speed = [.25]
+ last_key = [0.]
+ events = []
+ def key(code):
+ if code in (87,265): command[:] = [speed[0],0]
+ elif code in (83,264): command[:] = [-speed[0],0]
+ elif code in (65,263): command[:] = [0,.35]
+ elif code in (68,262): command[:] = [0,-.35]
+ elif code == 32: command[:] = 0
+ elif code in (61,334): speed[0] = min(.4,speed[0]+.05)
+ elif code in (45,333): speed[0] = max(.1,speed[0]-.05)
+ last_key[0] = time.monotonic()
+ events.append(dict(key=code,command=command.tolist(),time=float(sim.data.time)))
+ print('WASD/arrows: move, space: stop, +/-: speed; repeat presses to keep moving.',flush=True)
+ try:
+ with mujoco.viewer.launch_passive(sim.model,sim.data,key_callback=key) as viewer:
+ while viewer.is_running() and not sim.fallen and sim.data.time.6: command[:] = 0
+ sim.set_command(command[0],wz=command[1])
+ sim.step()
+ viewer.sync()
+ time.sleep(max(0,.02-(time.monotonic()-start)))
+ finally:
+ sim.set_command()
+ if output:
+ output=Path(output)
+ output.mkdir(parents=True,exist_ok=True)
+ (output/'teleop_check.json').write_text(json.dumps(dict(events=events,
+ final_position=sim.data.qpos[:3].tolist(),fallen=bool(sim.fallen),
+ simulated_seconds=float(sim.data.time)),indent=2),encoding='utf-8')
+ sim.close()
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/scene.py b/src/humanoid/humanoid_visual_patrol/patrol/scene.py
new file mode 100644
index 0000000000..df750e8a32
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/scene.py
@@ -0,0 +1,112 @@
+"""Generate indoor geometry without changing the upstream robot assets."""
+from pathlib import Path
+import tempfile
+import xml.etree.ElementTree as ET
+
+import mujoco
+import numpy as np
+
+from .locomotion import Walker
+
+
+def build_scene(assets, seed=0):
+ asset_dir = Path(assets).resolve() / 'resources/robots/g1_description'
+ tree = ET.parse(asset_dir/'g1_12dof.xml')
+ root = tree.getroot()
+ root.find('compiler').set('meshdir', (asset_dir/'meshes').as_posix())
+ world = root.find('worldbody')
+ pelvis = world.find("body[@name='pelvis']")
+ for geom in pelvis.iter('geom'):
+ geom.set('group', '1')
+ ET.SubElement(pelvis, 'camera', name='front', pos='0.16 0 0.35',
+ xyaxes='0 -1 0 0.35 0 0.94', fovy='75')
+ visual = ET.SubElement(root, 'visual')
+ ET.SubElement(visual, 'global', offwidth='640', offheight='480')
+ ET.SubElement(visual, 'headlight', ambient='0.4 0.4 0.4', diffuse='0.6 0.6 0.6')
+ rng = np.random.default_rng(seed)
+ brightness = float(rng.uniform(.6, .95))
+ ET.SubElement(world, 'light', pos='3 0 4', diffuse=f'{brightness} {brightness} {brightness}')
+ ET.SubElement(world, 'geom', name='floor', type='plane', size='15 15 .1', rgba='.65 .65 .60 1')
+ boxes = [('left_wall', [4, 2, 1], [6, .12, 1]),
+ ('right_wall', [4, -2, 1], [6, .12, 1]),
+ ('back_wall', [-2, 0, 1], [.12, 2, 1]),
+ ('end_wall', [10, 0, 1], [.12, 2, 1]),
+ ('door_left', [5, 1.65, .9], [.12, .35, .9]),
+ ('door_right', [5, -1.65, .9], [.12, .35, .9]),
+ ('cabinet', [3, .95 + float(rng.uniform(-.15, .15)), .45], [.3, .3, .45])]
+ for name, pos, size in boxes:
+ rgba = [*rng.uniform(.25, .75, 3), 1]
+ ET.SubElement(world, 'geom', name=name, type='box',
+ pos=' '.join(map(str, pos)), size=' '.join(map(str, size)),
+ rgba=' '.join(map(str, rgba)))
+ ET.SubElement(world, 'geom', name='inspection_marker', type='box',
+ pos='9.84 0 1', size='.02 .35 .35', rgba='.1 .7 .8 1',
+ contype='0', conaffinity='0')
+ return ET.tostring(root, encoding='unicode')
+
+
+class IndoorSimulator(Walker):
+ def __init__(self, assets, seed=0):
+ # MuJoCo 3.2 parses an XML file; generated files remain outside the repo.
+ with tempfile.TemporaryDirectory(prefix='humanoid_scene_') as folder:
+ path = Path(folder)/'scene.xml'
+ path.write_text(build_scene(assets, seed), encoding='utf-8')
+ super().__init__(assets, path)
+ self.renderer = None
+ self.floor_id = self.model.geom('floor').id
+ self.rng = np.random.default_rng(seed)
+ self.odom = np.zeros(3)
+ self._last_yaw = self.yaw
+
+ def step(self):
+ super().step()
+ # Simulated velocity/gyro measurements, integrated independently of position.
+ dt = self.model.opt.timestep*self.config['control_decimation']
+ yaw = self.yaw
+ rotation = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])
+ body_velocity = rotation @ self.data.qvel[:2]
+ body_velocity += self.rng.normal(0, .003, 2)
+ dyaw = (yaw-self._last_yaw+np.pi)%(2*np.pi)-np.pi
+ self._last_yaw = yaw
+ self.odom[2] += dyaw + self.rng.normal(0, .0002)
+ c, s = np.cos(self.odom[2]), np.sin(self.odom[2])
+ self.odom[:2] += np.array([[c, -s], [s, c]])@body_velocity*dt
+
+ def image(self, labels=False, depth=False):
+ if self.renderer is None:
+ self.renderer = mujoco.Renderer(self.model, height=96, width=128)
+ r = self.renderer
+ r.disable_segmentation_rendering()
+ r.disable_depth_rendering()
+ r.update_scene(self.data, camera='front')
+ rgb = r.render().copy()
+ if labels:
+ r.enable_segmentation_rendering()
+ segmentation = r.render().copy()
+ r.disable_segmentation_rendering()
+ return rgb, (segmentation[:, :, 0] == self.floor_id).astype(np.uint8)
+ if depth:
+ r.enable_depth_rendering()
+ result = r.render().copy()
+ r.disable_depth_rendering()
+ return rgb, result
+ return rgb
+
+ def scan(self, count=180):
+ origin = self.data.qpos[:3].copy()
+ origin[2] = .65
+ ranges = []
+ geom_id = np.zeros(1, dtype=np.int32)
+ # Only static geometry: no robot self returns. Angle frame is level base_link.
+ for angle in np.linspace(-np.pi, np.pi, count, endpoint=False):
+ a = angle + self.yaw
+ distance = mujoco.mj_ray(self.model, self.data, origin,
+ np.array([np.cos(a), np.sin(a), 0.]), np.array([1,0,0,0,0,0], dtype=np.uint8), 1,
+ self.model.body('pelvis').id, geom_id)
+ ranges.append(float(np.clip(distance if distance >= 0 else 8., .05, 8.)))
+ return ranges
+
+ def close(self):
+ if self.renderer:
+ self.renderer.close()
+ self.renderer = None
diff --git a/src/humanoid/humanoid_visual_patrol/patrol/vision.py b/src/humanoid/humanoid_visual_patrol/patrol/vision.py
new file mode 100644
index 0000000000..b0d706f474
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/patrol/vision.py
@@ -0,0 +1,89 @@
+"""Separate perception and direct-image control models."""
+import numpy as np
+import torch
+from torch import nn
+
+
+class FloorNet(nn.Module):
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.Conv2d(3, 16, 5, padding=2), nn.ReLU(),
+ nn.Conv2d(16, 16, 3, padding=1), nn.ReLU(), nn.Conv2d(16, 1, 1))
+
+ def forward(self, image):
+ return self.net(image)
+
+
+class ImageController(nn.Module):
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.Conv2d(3, 16, 5, stride=2, padding=2), nn.ReLU(),
+ nn.Conv2d(16, 32, 3, stride=2, padding=1), nn.ReLU(),
+ nn.Conv2d(32, 32, 3, stride=2, padding=1), nn.ReLU(),
+ nn.Flatten(), nn.Linear(32*12*16, 64), nn.ReLU(), nn.Linear(64, 2))
+
+ def forward(self, image):
+ return self.net(image)
+
+
+class MaskController(nn.Module):
+ """Learned velocity policy operating on a semantic floor mask."""
+ def __init__(self):
+ super().__init__()
+ self.net = nn.Sequential(nn.AdaptiveAvgPool2d((12,16)),nn.Flatten(),
+ nn.Linear(192,64),nn.ReLU(),nn.Linear(64,32),nn.ReLU(),nn.Linear(32,2))
+
+ def forward(self, mask):
+ return self.net(mask)
+
+
+def mask_command(mask):
+ """Reactive floor-following teacher, with no global route/position input.
+
+ Scores candidate image corridors; low front clearance reduces speed.
+ This is a heuristic teacher, not a globally optimal navigation planner.
+ """
+ h, w = mask.shape
+ band = mask[int(h*.48):int(h*.82)]
+ scores = np.array([band[:, max(0, x-10):min(w, x+11)].mean()
+ for x in range(12, w-12, 4)])
+ centers = np.arange(12, w-12, 4)
+ scores -= .20*np.abs(centers-w/2)/(w/2)
+ best = centers[np.argmax(scores)]
+ yaw = float(np.clip((w/2-best)/(w/2)*1.2, -.45, .45))
+ clearance = float(mask[int(h*.55):int(h*.85), int(w*.4):int(w*.6)].mean())
+ speed = .28*float(np.clip((clearance-.15)/.6, 0, 1))*(1-.6*abs(yaw))
+ return np.array([speed, yaw], dtype=np.float32)
+
+
+def tensor_image(image):
+ return torch.from_numpy(image.copy()).permute(2, 0, 1).float().unsqueeze(0)/255.
+
+
+class VisionPolicy:
+ def __init__(self, weights, mode):
+ self.mode = mode
+ self.net = FloorNet() if mode == 'perception' else ImageController()
+ self.net.load_state_dict(torch.load(weights, map_location='cpu', weights_only=True))
+ self.net.eval()
+ if mode == 'perception':
+ from pathlib import Path
+ self.controller = MaskController()
+ self.controller.load_state_dict(torch.load(Path(weights).with_name('mask_controller.pt'),
+ map_location='cpu',weights_only=True))
+ self.controller.eval()
+
+ def command(self, image):
+ with torch.inference_mode():
+ output = self.net(tensor_image(image))
+ if self.mode == 'perception':
+ mask = (output.sigmoid()[0, 0].numpy()>.5).astype(np.float32)
+ with torch.inference_mode():
+ command = self.controller(torch.from_numpy(mask)[None,None])[0].numpy()
+ # A learned controller can extrapolate a forward command even when
+ # segmentation detects no floor. Stop on this unambiguous failure.
+ if not mask.any():
+ command = np.zeros(2, dtype=np.float32)
+ return np.clip(command,[0,-.45],[.3,.45]), mask
+ command = output[0].numpy()
+ return np.clip(command, [0, -.45], [.3, .45]), None
diff --git a/src/humanoid/humanoid_visual_patrol/requirements-dev.txt b/src/humanoid/humanoid_visual_patrol/requirements-dev.txt
new file mode 100644
index 0000000000..26b77f68be
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/requirements-dev.txt
@@ -0,0 +1,2 @@
+-r requirements.txt
+pytest
diff --git a/src/humanoid/humanoid_visual_patrol/requirements.txt b/src/humanoid/humanoid_visual_patrol/requirements.txt
new file mode 100644
index 0000000000..f55b4da02e
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/requirements.txt
@@ -0,0 +1,6 @@
+mujoco==3.2.0
+numpy>=1.24
+torch>=2.0
+PyYAML>=6
+Pillow>=10
+imageio>=2.30
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py b/src/humanoid/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py
new file mode 100644
index 0000000000..3759f3493a
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/evaluate_near_obstacle.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+"""Measure the deployed image controller near the end wall in MuJoCo."""
+import argparse
+import json
+from pathlib import Path
+import sys
+
+import mujoco
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
+from patrol.scene import IndoorSimulator
+from patrol.vision import VisionPolicy
+
+
+def run(assets, weights, distance):
+ sim = IndoorSimulator(assets, 7)
+ try:
+ # End wall's near surface is x=9.88. Distance is pelvis to wall,
+ # not foot or body clearance. Settle before enabling the policy.
+ sim.data.qpos[0] = 9.88 - distance
+ mujoco.mj_forward(sim.model, sim.data)
+ policy = VisionPolicy(Path(weights) / 'end2end.pt', 'end2end')
+ for _ in range(100):
+ sim.set_command(0, 0, 0)
+ sim.step()
+ if sim.fallen:
+ return dict(distance_m=distance, settled=False, fallen=True)
+ start = sim.data.qpos[:3].copy()
+ commands = []
+ contacts = 0
+ previous = np.zeros(2)
+ for tick in range(150):
+ if tick % 10 == 0:
+ command, _ = policy.command(sim.image())
+ previous = .6 * previous + .4 * command
+ sim.set_command(float(previous[0]), wz=float(previous[1]))
+ commands.append(command.tolist())
+ sim.step()
+ for contact in sim.data.contact:
+ names = [mujoco.mj_id2name(sim.model, mujoco.mjtObj.mjOBJ_GEOM, int(g)) or ''
+ for g in (contact.geom1, contact.geom2)]
+ if 'end_wall' in names:
+ contacts += 1
+ break
+ if sim.fallen:
+ break
+ return dict(distance_m=distance, settled=True, start_position=start.tolist(),
+ final_position=sim.data.qpos[:3].tolist(), fallen=bool(sim.fallen),
+ wall_contact_steps=contacts, commands=commands)
+ finally:
+ sim.close()
+
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('--assets', required=True)
+ parser.add_argument('--weights', default=str(Path(__file__).resolve().parents[1] / 'models'))
+ parser.add_argument('--output', required=True)
+ args = parser.parse_args()
+ results = [run(args.assets, args.weights, d) for d in (1.5, 1.0, .6)]
+ Path(args.output).write_text(json.dumps(results, indent=2) + '\n', encoding='utf-8')
+ print(json.dumps(results, indent=2))
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py b/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py
new file mode 100644
index 0000000000..71f10aa86b
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/fetch_assets.py
@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+"""Fetch the exact upstream model/policy revision; never overwrite an existing path."""
+import argparse
+from pathlib import Path
+import subprocess
+
+REVISION = '276801e46c5d433564f24658bac64f254b7d2d4b'
+URL = 'https://github.com/unitreerobotics/unitree_rl_gym.git'
+
+
+def main():
+ parser = argparse.ArgumentParser(description=__doc__)
+ parser.add_argument('destination',type=Path)
+ args = parser.parse_args()
+ target = args.destination.resolve()
+ if target.exists():
+ raise SystemExit('Destination exists; use a new directory or validate the existing revision manually.')
+ subprocess.run(['git','clone','--filter=blob:none','--no-checkout',URL,str(target)],check=True)
+ subprocess.run(['git','-C',str(target),'sparse-checkout','set','deploy','resources/robots/g1_description'],check=True)
+ subprocess.run(['git','-C',str(target),'checkout','--detach',REVISION],check=True)
+ print(target)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/neural_planner.py b/src/humanoid/humanoid_visual_patrol/scripts/neural_planner.py
new file mode 100755
index 0000000000..3990a8c109
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/neural_planner.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+"""Plan on the live SLAM map with a learned tie breaker; execute via move_base."""
+import json
+import subprocess
+import threading
+
+import actionlib
+import rospy
+import tf
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import OccupancyGrid, Path
+from std_msgs.msg import String
+from move_base_msgs.msg import MoveBaseAction,MoveBaseGoal
+
+
+def main():
+ rospy.init_node('neural_patrol_planner')
+ latest_map=[None];pending=[None]
+ rospy.Subscriber('/map',OccupancyGrid,lambda msg:latest_map.__setitem__(0,msg),queue_size=1)
+ rospy.Subscriber('/move_base_simple/goal',PoseStamped,lambda msg:pending.__setitem__(0,msg),queue_size=1)
+ result_pub=rospy.Publisher('/neural_patrol/result',String,queue_size=1,latch=True)
+ path_pub=rospy.Publisher('/neural_patrol/path',Path,queue_size=1,latch=True)
+ listener=tf.TransformListener()
+ client=actionlib.SimpleActionClient('/move_base',MoveBaseAction)
+ while not rospy.is_shutdown():
+ if pending[0] is None or latest_map[0] is None:
+ rospy.rostime.wallsleep(.1);continue
+ goal=pending[0];pending[0]=None
+ try:
+ if goal.header.frame_id!='map': raise ValueError('Goal must use map frame')
+ listener.waitForTransform('map','base_link',rospy.Time(0),rospy.Duration(5))
+ position,_=listener.lookupTransform('map','base_link',rospy.Time(0))
+ grid=latest_map[0]
+ request=dict(map=list(grid.data),width=grid.info.width,height=grid.info.height,
+ resolution=grid.info.resolution,origin=[grid.info.origin.position.x,grid.info.origin.position.y],
+ start=list(position[:2]),goal=[goal.pose.position.x,goal.pose.position.y])
+ command=[rospy.get_param('~sim_python'),rospy.get_param('~sim_entry'),
+ '--mode','plan_stdio','--assets',rospy.get_param('~assets'),
+ '--weights',rospy.get_param('~weights')]
+ output=subprocess.run(command,input=json.dumps(request),stdout=subprocess.PIPE,
+ stderr=subprocess.PIPE,universal_newlines=True,timeout=30,check=True)
+ report=json.loads(output.stdout)
+ report['request_id']=goal.header.stamp.to_nsec()
+ path=Path();path.header.frame_id='map';path.header.stamp=rospy.Time.now()
+ for x,y in report['waypoints']:
+ pose=PoseStamped();pose.header=path.header
+ pose.pose.position.x=x;pose.pose.position.y=y;pose.pose.orientation.w=1
+ path.poses.append(pose)
+ path_pub.publish(path)
+ if not client.wait_for_server(rospy.Duration(10)):raise RuntimeError('move_base unavailable')
+ report['execution']=[]
+ for pose in path.poses:
+ target=MoveBaseGoal();target.target_pose=pose
+ target.target_pose.header.stamp=rospy.Time.now()
+ client.send_goal(target)
+ completed=client.wait_for_result(rospy.Duration(45))
+ state=client.get_state()
+ report['execution'].append(dict(goal=[pose.pose.position.x,pose.pose.position.y],state=state))
+ if not completed or state!=3:
+ client.cancel_goal();break
+ report['success']=len(report['execution'])==len(path.poses) and all(p['state']==3 for p in report['execution'])
+ result_pub.publish(String(data=json.dumps(report)))
+ except Exception as exc:
+ client.cancel_goal()
+ detail=getattr(exc,'stderr',None) or str(exc)
+ rospy.logerr('Neural planning failed: %s',detail)
+ result_pub.publish(String(data=json.dumps(dict(success=False,error=detail,request_id=goal.header.stamp.to_nsec()))))
+
+
+if __name__=='__main__':
+ main()
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py b/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py
new file mode 100755
index 0000000000..5dd2c0c016
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/ros_bridge.py
@@ -0,0 +1,108 @@
+#!/usr/bin/env python3
+"""ROS Noetic bridge. Simulation uses an isolated interpreter over stdio.
+
+The bridge only consumes integrated noisy odometry, never ground-truth position.
+On WSL, an explicit Windows Python executable can run the MuJoCo process.
+"""
+import json
+import math
+import os
+import subprocess
+import time
+import base64
+
+import rospy
+import tf
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan, JointState, Image
+from rosgraph_msgs.msg import Clock
+
+
+def main():
+ rospy.init_node('humanoid_sim_bridge')
+ interpreter = rospy.get_param('~sim_python')
+ entry = rospy.get_param('~sim_entry')
+ assets = rospy.get_param('~assets')
+ seed = str(rospy.get_param('~seed',7))
+ controller=rospy.get_param('~task','navigation')
+ weights=rospy.get_param('~weights','models')
+ duration=float(rospy.get_param('~seconds',45))
+ real_time_factor = float(rospy.get_param('~real_time_factor',1.0))
+ command = [0.,0.,0.]
+ last_command = [0.]
+ def callback(msg):
+ command[:] = [msg.linear.x,msg.linear.y,msg.angular.z]
+ last_command[0] = time.monotonic()
+ rospy.Subscriber('/cmd_vel',Twist,callback,queue_size=1)
+ scan_pub = rospy.Publisher('/scan',LaserScan,queue_size=2)
+ odom_pub = rospy.Publisher('/odom',Odometry,queue_size=2)
+ clock_pub = rospy.Publisher('/clock',Clock,queue_size=2)
+ joint_pub = rospy.Publisher('/joint_states',JointState,queue_size=2)
+ image_pub = rospy.Publisher('/camera/image_raw',Image,queue_size=1)
+ broadcaster = tf.TransformBroadcaster()
+ child = subprocess.Popen([interpreter,entry,'--mode','bridge','--assets',assets,'--seed',seed,
+ '--controller',controller,'--weights',weights],
+ stdin=subprocess.PIPE,stdout=subprocess.PIPE,universal_newlines=True,bufsize=1)
+ try:
+ while not rospy.is_shutdown():
+ begin = time.monotonic()
+ if time.monotonic()-last_command[0] > .8:
+ command[:] = [0.,0.,0.]
+ child.stdin.write(json.dumps({'command':command})+'\n')
+ child.stdin.flush()
+ line = child.stdout.readline()
+ if not line:
+ raise RuntimeError('Simulator process exited')
+ state = json.loads(line)
+ stamp = rospy.Time.from_sec(state['time'])
+ clock_pub.publish(Clock(clock=stamp))
+ x,y,yaw = state['odom']
+ quaternion = tf.transformations.quaternion_from_euler(0,0,yaw)
+ broadcaster.sendTransform((x,y,0),quaternion,stamp,'base_link','odom')
+ broadcaster.sendTransform((0,0,.65),(0,0,0,1),stamp,'laser','base_link')
+ odom = Odometry()
+ odom.header.stamp,odom.header.frame_id,odom.child_frame_id = stamp,'odom','base_link'
+ odom.pose.pose.position.x,odom.pose.pose.position.y = x,y
+ odom.pose.pose.orientation.x,odom.pose.pose.orientation.y,odom.pose.pose.orientation.z,odom.pose.pose.orientation.w = quaternion
+ # Velocity from successive integrated odometry is in the body frame.
+ vx,vy = state['velocity']
+ odom.twist.twist.linear.x = math.cos(yaw)*vx+math.sin(yaw)*vy
+ odom.twist.twist.linear.y = -math.sin(yaw)*vx+math.cos(yaw)*vy
+ odom.twist.twist.angular.z = state['angular_velocity']
+ odom_pub.publish(odom)
+ scan = LaserScan()
+ scan.header.stamp,scan.header.frame_id = stamp,'laser'
+ scan.angle_min = -math.pi
+ scan.angle_increment = 2*math.pi/len(state['scan'])
+ scan.angle_max = scan.angle_min+(len(state['scan'])-1)*scan.angle_increment
+ scan.range_min,scan.range_max,scan.scan_time = .05,8.,.2
+ scan.ranges = state['scan']
+ scan_pub.publish(scan)
+ joints=JointState()
+ joints.header.stamp=stamp
+ joints.name=state['joint_names']; joints.position=state['joints']
+ joint_pub.publish(joints)
+ image=Image()
+ image.header.stamp=stamp; image.header.frame_id='front_camera'
+ image.height,image.width=state['image_shape'][:2]
+ image.encoding='rgb8'; image.step=image.width*3
+ image.data=base64.b64decode(state['image'])
+ image_pub.publish(image)
+ if state['fallen']:
+ rospy.logerr('Robot fell; stopping simulator')
+ break
+ if controller!='navigation' and state['time']>=duration:
+ break
+ time.sleep(max(0,.2/real_time_factor-(time.monotonic()-begin)))
+ finally:
+ try:
+ child.stdin.write('{"stop":true}\n')
+ child.stdin.flush()
+ child.wait(timeout=5)
+ except (OSError,subprocess.TimeoutExpired):
+ child.terminate()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/run_navigation.sh b/src/humanoid/humanoid_visual_patrol/scripts/run_navigation.sh
new file mode 100644
index 0000000000..30b6ed3192
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/run_navigation.sh
@@ -0,0 +1,22 @@
+#!/usr/bin/env bash
+# Usage: bash scripts/run_navigation.sh SIM_PYTHON ASSETS [TASK] [WEIGHTS] [OUTPUT]
+set -e
+MODULE_DIR="$(cd -- "$(dirname -- "$0")/.." && pwd)"
+source /opt/ros/noetic/setup.bash
+export ROS_MASTER_URI="${ROS_MASTER_URI:-http://localhost:11331}"
+export ROS_PACKAGE_PATH="$MODULE_DIR:$ROS_PACKAGE_PATH"
+SIM_PYTHON="${1:?Provide the simulator Python executable}"
+ASSETS="${2:?Provide the unitree_rl_gym path}"
+TASK="${3:-navigation}"
+WEIGHTS="${4:-$MODULE_DIR/models}"
+OUTPUT="${5:-$MODULE_DIR/results}"
+ENTRY="$MODULE_DIR/main.py"
+if [[ "$SIM_PYTHON" == *.exe ]]; then
+ ENTRY="$(wslpath -w "$ENTRY")"
+ ASSETS="$(wslpath -w "$ASSETS")"
+ WEIGHTS="$(wslpath -w "$WEIGHTS")"
+ OUTPUT="$(wslpath -w "$OUTPUT")"
+fi
+exec roslaunch -p "${ROS_MASTER_URI##*:}" humanoid_visual_patrol main.launch \
+ sim_python:="$SIM_PYTHON" sim_entry:="$ENTRY" assets:="$ASSETS" \
+ task:="$TASK" weights:="$WEIGHTS" output:="$OUTPUT"
diff --git a/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py b/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py
new file mode 100755
index 0000000000..c1112df063
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/scripts/task_runner.py
@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+"""Launch standalone simulation/learning tasks through a ROS launch file."""
+import subprocess
+import rospy
+
+
+def main():
+ rospy.init_node('humanoid_task_runner')
+ args = [rospy.get_param('~sim_python'),rospy.get_param('~sim_entry'),
+ '--assets',rospy.get_param('~assets'),'--mode',rospy.get_param('~task'),
+ '--weights',rospy.get_param('~weights'),'--output',rospy.get_param('~output'),
+ '--seconds',str(rospy.get_param('~seconds',45))]
+ child = subprocess.Popen(args)
+ try:
+ while child.poll() is None and not rospy.is_shutdown():
+ rospy.rostime.wallsleep(.1)
+ if child.poll() not in (None,0):
+ raise RuntimeError('Simulation task failed with exit code %s'%child.returncode)
+ finally:
+ if child.poll() is None:
+ child.terminate()
+ child.wait(timeout=10)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py b/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py
new file mode 100644
index 0000000000..4f39435f20
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/tests/test_contracts.py
@@ -0,0 +1,68 @@
+import os
+from pathlib import Path
+import sys
+
+import numpy as np
+import pytest
+
+sys.path.insert(0,str(Path(__file__).resolve().parents[1]))
+from patrol.vision import mask_command
+
+
+def test_deployed_perception_stops_when_segmentation_finds_no_floor():
+ import torch
+ from patrol.vision import VisionPolicy
+ policy = VisionPolicy(Path(__file__).resolve().parents[1] / 'models/perception.pt', 'perception')
+ class NoFloor(torch.nn.Module):
+ def forward(self, image):
+ return torch.full((1, 1, 96, 128), -100.0)
+ policy.net = NoFloor()
+ command, mask = policy.command(np.zeros((96,128,3), dtype=np.uint8))
+ assert not mask.any()
+ assert np.array_equal(command, [0, 0])
+
+
+def test_no_floor_does_not_command_forward_motion():
+ assert mask_command(np.zeros((96,128),dtype=np.uint8))[0] == 0
+
+
+def test_unilateral_floor_steers_into_visible_free_space():
+ left = np.zeros((96,128),dtype=np.uint8)
+ left[:,:60] = 1
+ assert mask_command(left)[1] > 0
+ assert mask_command(left[:,::-1])[1] < 0
+
+
+def test_bad_neural_estimates_cannot_change_primary_astar_optimal_cost():
+ from patrol.planning import astar,distances
+ rng=np.random.default_rng(93)
+ grid=np.zeros((16,16),dtype=bool)
+ grid[2:14,7]=True
+ grid[10,7]=False
+ start,goal=(4,3),(4,12)
+ expected=distances(grid,goal)[start]
+ route,_=astar(grid,start,goal,rng.normal(0,10000,grid.shape))
+ assert len(route)-1==expected
+ assert all(not grid[p] for p in route)
+
+
+def test_planner_reports_unreachable_goal():
+ from patrol.planning import astar
+ grid=np.zeros((10,10),dtype=bool);grid[5,:]=True
+ with pytest.raises(ValueError,match='No path'):
+ astar(grid,(2,2),(8,8))
+
+
+@pytest.mark.skipif(not os.environ.get('HUMANOID_ASSETS'),reason='set HUMANOID_ASSETS for real physics checks')
+def test_real_robot_stays_upright_and_scan_excludes_body():
+ from patrol.scene import IndoorSimulator
+ sim = IndoorSimulator(os.environ['HUMANOID_ASSETS'])
+ try:
+ # Robot starts 1.88m from side walls; there must be no 7cm self-return.
+ assert min(sim.scan()) > 1.0
+ result = sim.smoke_test()
+ assert not result['fallen']
+ assert result['displacement_m'] > 1
+ assert result['final_yaw_rad'] > .3
+ finally:
+ sim.close()
diff --git a/src/humanoid/humanoid_visual_patrol/third_party_license.md b/src/humanoid/humanoid_visual_patrol/third_party_license.md
new file mode 100644
index 0000000000..9ad8f6dbb0
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/third_party_license.md
@@ -0,0 +1,23 @@
+BSD 3-Clause License
+Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics")
+All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+3. Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/src/humanoid/humanoid_visual_patrol/third_party_notices.md b/src/humanoid/humanoid_visual_patrol/third_party_notices.md
new file mode 100644
index 0000000000..01dd8c97e2
--- /dev/null
+++ b/src/humanoid/humanoid_visual_patrol/third_party_notices.md
@@ -0,0 +1,20 @@
+# Third-party sources
+
+The G1 MJCF/STL assets, pretrained `motion.pt`, policy observation convention,
+PD gains and action scaling originate from
+[Unitree RL Gym](https://github.com/unitreerobotics/unitree_rl_gym), revision
+`276801e46c5d433564f24658bac64f254b7d2d4b`, BSD-3-Clause.
+The upstream copyright and license are preserved in [third_party_license.md](third_party_license.md).
+Assets are fetched separately and are not duplicated into this module.
+
+`patrol/locomotion.py` adapts the upstream deployment equations and observation
+ordering. The pretrained walking policy is not an original training result.
+
+MuJoCo supplies physical simulation, rendering and ray queries. ROS Noetic
+gmapping and move_base supply SLAM and classical navigation. These components
+retain their respective upstream licenses; no claim of a new SLAM algorithm is made.
+
+FloorNet, MaskController, ImageController and CostNet, the training data generator, visual corridor
+controller, indoor scene generator, ROS stdio bridge and evaluation code were
+created for this project with AI assistance. Training weights under `models/`
+were produced by this module's training script.