Robocon/RC_WheelLeg
4
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[real] 整理 ROS 2 v2 里程计导航联调

Browse Source
This commit is contained in:
eastzio
2026-07-27 17:38:59 +08:00
parent dc7a2c2a67
commit 765173676f
31 changed files with 520049 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
05_software/real/sim2real_ros2_v2/tools/pcd_nav_pygame.py
+1502
View File
File diff suppressed because it is too large Load Diff
05_software/real/sim2real_ros2_v2/tools/run_a_min_route_editor.bat
+7
View File
@@ -0,0 +1,7 @@
@echo off
setlocal
cd /d "%~dp0\.."
python .\tools\pcd_nav_pygame.py --pcd .\map\A_min.pcd --route-name A_min_route --route-map A_min --route-obstacle slalom --save-route .\map\routes\A_min\A_min_route.json --default-speed 0.35 --default-policy rough --default-tolerance 0.15
endlocal
05_software/real/sim2real_ros2_v2/tools/run_pcd_nav_map.bat
+7
View File
@@ -0,0 +1,7 @@
@echo off
setlocal
cd /d "%~dp0\.."
python .\tools\pcd_nav_pygame.py --pcd .\map\map_b.pcd --overlay-json .\tools\semantic_overlay_example.json
endlocal
05_software/real/sim2real_ros2_v2/tools/semantic_overlay_example.json
+52
View File
@@ -0,0 +1,52 @@
{
"elements": [
{
"name": "wall_01",
"label": "wall",
"type": "rect",
"x": 1.8,
"y": -7.0,
"length": 1.0,
"width": 0.05,
"yawDeg": 90.0
},
{
"name": "low_bar_01",
"label": "low_bar",
"type": "rect",
"x": 5.7,
"y": -9.0,
"length": 1.0,
"width": 0.03,
"yawDeg": 0.0
},
{
"name": "slalom_pole_01",
"label": "slalom_pole",
"type": "circle",
"x": 1.8,
"y": -10.5,
"radius": 0.05
},
{
"name": "stairs_area",
"label": "stairs",
"type": "rect",
"x": 2.4,
"y": -3.5,
"length": 2.4,
"width": 1.2,
"yawDeg": 0.0
},
{
"name": "ramp_area",
"label": "ramp",
"type": "rect",
"x": 5.2,
"y": -5.2,
"length": 2.0,
"width": 3.0,
"yawDeg": 0.0
}
]
}
05_software/real/sim2real_ros2_v2/tools/win_web_debug/server.py
+3
View File
@@ -25,6 +25,9 @@ class Handler(SimpleHTTPRequestHandler):
self._json(200, data)
return
if self.path == "/api/map":
global LATEST_MAP
with STATE_LOCK:
LATEST_MAP = None
send_udp({"type": "map_request", "stamp": time.time()})
deadline = time.time() + 1.0
while time.time() < deadline:
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/app.js
+96 -10
View File
@@ -25,6 +25,7 @@ let goalPointsByName = {};
let defaultMissionName = '';
let defaultMissionGoals = [];
let routeAlignmentInfo = {};
let odomFallbackInfo = { active: false };
let activeNavPath = { goal_name: '', stage: 'idle', path_index: 0, points: [] };
const mapCanvas = $('map-canvas');
@@ -89,18 +90,37 @@ function highlightMode(mode) {
function updateTaskButtonState() {
const btn = $('btn-run-task');
if (!btn) return;
const odomBtn = $('btn-odom-task');
const odomStopBtn = $('btn-odom-stop');
const hasMission = Boolean(defaultMissionName);
const enabled = latestConnected && currentMode === 'NAV' && hasMission;
btn.disabled = !enabled;
btn.textContent = hasMission ? `Run ${defaultMissionName}` : 'Run Mission';
if (!hasMission) {
btn.title = 'No default mission loaded from backend';
return;
const runEnabled = latestConnected && currentMode === 'NAV' && hasMission;
const odomEnabled = latestConnected && hasMission;
if (btn) {
btn.disabled = !runEnabled;
btn.textContent = hasMission ? `Run ${defaultMissionName}` : 'Run Mission';
if (!hasMission) {
btn.title = 'No default mission loaded from backend';
} else {
btn.title = runEnabled
? `Run default mission ${defaultMissionName}`
: 'Available only in NAV mode while connected';
}
}
if (odomBtn) {
odomBtn.disabled = !odomEnabled;
odomBtn.title = hasMission
? 'Initialize map->odom from waypoint id1, switch to NAV, then run the current route'
: 'No default mission loaded from backend';
}
if (odomStopBtn) {
odomStopBtn.disabled = !(latestConnected && odomFallbackInfo.active);
odomStopBtn.title = odomFallbackInfo.active
? 'Stop pure odom fallback and stop simple_nav'
: 'Odom fallback is not active';
}
btn.title = enabled
? `Run default mission ${defaultMissionName}`
: 'Available only in NAV mode while connected';
}
function setText(id, text, cls) {
@@ -355,6 +375,17 @@ function formatRouteAlignment(info) {
return `${applied} hits=${hits} ${reason}`;
}
function formatOdomFallback(info) {
if (!info || !info.active) return 'inactive';
const anchor = info.anchor || 'id1';
const pose = info.map_pose || {};
const x = Number.isFinite(pose.x) ? Number(pose.x).toFixed(2) : '--';
const y = Number.isFinite(pose.y) ? Number(pose.y).toFixed(2) : '--';
const yaw = Number.isFinite(pose.yaw_deg) ? Number(pose.yaw_deg).toFixed(1) : '--';
const handoff = info.handoff_pending ? ' handoff pending' : '';
return `${anchor} x=${x} y=${y} yaw=${yaw}deg${handoff}`;
}
function renderTaskGoals() {
const list = $('task-goals-list');
if (!list) return;
@@ -392,6 +423,7 @@ function applyMapMetadata(data) {
defaultMissionName = data.default_mission_name || '';
defaultMissionGoals = Array.isArray(data.default_mission_goals) ? data.default_mission_goals : [];
routeAlignmentInfo = data.route_alignment || {};
odomFallbackInfo = data.odom_fallback || odomFallbackInfo;
activeNavPath = data.nav_path || activeNavPath;
currentGoalIndex = currentGoalName
? defaultMissionGoals.findIndex(goal => goal.name === currentGoalName)
@@ -401,6 +433,11 @@ function applyMapMetadata(data) {
setText('d-task-mission', defaultMissionName || '--', defaultMissionName ? 'ok' : '');
setText('d-task-goals', String(defaultMissionGoals.length || 0), defaultMissionGoals.length ? 'ok' : '');
setText('d-route-align', formatRouteAlignment(routeAlignmentInfo), routeAlignmentInfo.enabled ? 'ok' : 'warn');
setText(
'd-odom-fallback',
formatOdomFallback(odomFallbackInfo),
odomFallbackInfo.handoff_pending ? 'warn' : odomFallbackInfo.active ? 'active' : '',
);
updateTaskButtonState();
renderTaskGoals();
}
@@ -527,6 +564,14 @@ function applyState(data) {
syncGoalMarkerFromNavStatus(data.nav_status || '');
const nav = data.nav || {};
if (nav.odom_fallback) {
odomFallbackInfo = nav.odom_fallback;
setText(
'd-odom-fallback',
formatOdomFallback(odomFallbackInfo),
odomFallbackInfo.handoff_pending ? 'warn' : odomFallbackInfo.active ? 'active' : '',
);
}
if (nav.pose) {
latestPose = nav.pose;
setText('d-nav-pose', `x=${nav.pose.x.toFixed(2)} y=${nav.pose.y.toFixed(2)} yaw=${(nav.pose.yaw * 57.2958).toFixed(1)}deg`);
@@ -605,6 +650,47 @@ $('btn-estop').onclick = async () => {
}
};
$('btn-refresh-map').onclick = fetchMap;
$('btn-odom-task').onclick = async () => {
if (!latestConnected) {
appendEvent('ODOM', 'bridge not connected', 'warn');
updateTaskButtonState();
return;
}
if (!defaultMissionName || !defaultMissionGoals.length) {
appendEvent('ODOM', 'no route mission loaded', 'warn');
return;
}
const start = defaultMissionGoals[0];
const ok = confirm(
`Run odom fallback mission ${defaultMissionName}?\n\n` +
`Place the robot at waypoint id1 before confirming:\n` +
`x=${Number(start.x).toFixed(3)} y=${Number(start.y).toFixed(3)} yaw=${Number(start.yaw_deg || 0).toFixed(1)}deg`
);
if (!ok) return;
setLastGoalByName(start.name);
drawMap();
const result = await post({ type: 'odom_task' });
if (result.ok) {
highlightMode('NAV');
appendEvent('ODOM', `init at ${start.name}, run ${defaultMissionName}`, 'ok');
setTimeout(fetchMap, 400);
}
};
$('btn-odom-stop').onclick = async () => {
if (!latestConnected) {
appendEvent('ODOM', 'bridge not connected', 'warn');
updateTaskButtonState();
return;
}
const result = await post({ type: 'odom_stop' });
if (result.ok) {
odomFallbackInfo = { active: false };
setText('d-odom-fallback', formatOdomFallback(odomFallbackInfo), '');
updateTaskButtonState();
appendEvent('ODOM', 'fallback stopped', 'ok');
setTimeout(fetchMap, 400);
}
};
$('btn-run-task').onclick = async () => {
if (!latestConnected) {
appendEvent('TASK', 'bridge not connected', 'warn');
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/index.html
+5 -2
View File
@@ -4,7 +4,7 @@
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>sim2real ROS2 Console</title>
<link rel="stylesheet" href="style.css?v=mission-sync-2">
<link rel="stylesheet" href="style.css?v=odom-fallback-1">
</head>
<body>
<header class="glass-panel top-bar">
@@ -56,6 +56,7 @@
<div class="diag-row"><span class="diag-label">active goal</span><span class="diag-value" id="d-active-goal">--</span></div>
<div class="diag-row"><span class="diag-label">goal count</span><span class="diag-value" id="d-task-goals">--</span></div>
<div class="diag-row"><span class="diag-label">route align</span><span class="diag-value" id="d-route-align">--</span></div>
<div class="diag-row"><span class="diag-label">odom fallback</span><span class="diag-value" id="d-odom-fallback">--</span></div>
</div>
<div class="panel-section flex-1">
@@ -70,6 +71,8 @@
<h2 class="panel-title">PCD 2D Nav</h2>
<div class="map-actions">
<button class="btn" id="btn-refresh-map">Refresh Map</button>
<button class="btn btn-odom" id="btn-odom-task" disabled>odom</button>
<button class="btn" id="btn-odom-stop" disabled>Exit odom</button>
<button class="btn btn-nav" id="btn-run-task" disabled>Run Mission</button>
<button class="btn" id="btn-record">Record Pose</button>
<button class="btn" id="btn-stop-nav">Stop Nav</button>
@@ -121,6 +124,6 @@
</div>
</div>
<script src="app.js?v=mission-sync-2"></script>
<script src="app.js?v=odom-fallback-1"></script>
</body>
</html>
05_software/real/sim2real_ros2_v2/tools/win_web_debug/static/style.css
+1
View File
@@ -105,6 +105,7 @@ body {
.btn-remote { background: rgba(48,209,88,0.2); border-color: rgba(48,209,88,0.4); color: #8deda7; }
.btn-web { background: rgba(10,132,255,0.2); border-color: rgba(10,132,255,0.4); color: #82c4ff; }
.btn-nav { background: rgba(255,214,10,0.2); border-color: rgba(255,214,10,0.4); color: #ffe680; }
.btn-odom { background: rgba(255,159,10,0.22); border-color: rgba(255,159,10,0.45); color: #ffd199; }
.btn.active-mode { box-shadow: 0 0 0 2px white; }
.route-select {