Merge pull request 'feat: WebUI gamepad teleop panel (Issue #598)' (#603) from sl-webui/issue-598-gamepad-teleop into main
This commit is contained in:
commit
7e5f673f7d
314
ui/gamepad_panel.css
Normal file
314
ui/gamepad_panel.css
Normal file
@ -0,0 +1,314 @@
|
||||
/* gamepad_panel.css — Saltybot Gamepad Teleop (Issue #598) */
|
||||
|
||||
*, *::before, *::after { box-sizing: border-box; margin: 0; padding: 0; }
|
||||
|
||||
:root {
|
||||
--bg0: #050510;
|
||||
--bg1: #070712;
|
||||
--bg2: #0a0a1a;
|
||||
--bd: #0c2a3a;
|
||||
--bd2: #1e3a5f;
|
||||
--dim: #374151;
|
||||
--mid: #6b7280;
|
||||
--base: #9ca3af;
|
||||
--hi: #d1d5db;
|
||||
--cyan: #06b6d4;
|
||||
--green: #22c55e;
|
||||
--amber: #f59e0b;
|
||||
--red: #ef4444;
|
||||
}
|
||||
|
||||
body {
|
||||
font-family: 'Courier New', Courier, monospace;
|
||||
font-size: 12px;
|
||||
background: var(--bg0);
|
||||
color: var(--base);
|
||||
height: 100dvh;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
/* ── Header ── */
|
||||
#header {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
padding: 5px 12px;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
gap: 8px;
|
||||
flex-wrap: wrap;
|
||||
}
|
||||
.logo { color: #f97316; font-weight: bold; letter-spacing: .15em; font-size: 13px; flex-shrink: 0; }
|
||||
|
||||
#conn-dot {
|
||||
width: 8px; height: 8px; border-radius: 50%;
|
||||
background: var(--dim); flex-shrink: 0; transition: background .3s;
|
||||
}
|
||||
#conn-dot.connected { background: var(--green); }
|
||||
#conn-dot.error { background: var(--red); animation: blink 1s infinite; }
|
||||
|
||||
#gp-indicator {
|
||||
display: flex; align-items: center; gap: 5px;
|
||||
font-size: 10px; color: var(--mid);
|
||||
}
|
||||
#gp-dot {
|
||||
width: 8px; height: 8px; border-radius: 50%;
|
||||
background: var(--dim); flex-shrink: 0; transition: background .3s;
|
||||
}
|
||||
#gp-dot.active { background: var(--amber); }
|
||||
|
||||
@keyframes blink { 0%,100%{opacity:1} 50%{opacity:.3} }
|
||||
|
||||
#ws-input {
|
||||
background: var(--bg2); border: 1px solid var(--bd2); border-radius: 4px;
|
||||
color: #67e8f9; padding: 2px 7px; font-family: monospace; font-size: 11px; width: 185px;
|
||||
}
|
||||
#ws-input:focus { outline: none; border-color: var(--cyan); }
|
||||
|
||||
.hbtn {
|
||||
padding: 2px 8px; border-radius: 4px; border: 1px solid var(--bd2);
|
||||
background: var(--bg2); color: #67e8f9; font-family: monospace;
|
||||
font-size: 10px; font-weight: bold; cursor: pointer; transition: background .15s;
|
||||
white-space: nowrap;
|
||||
}
|
||||
.hbtn:hover { background: #0e4f69; }
|
||||
|
||||
.estop-btn {
|
||||
border-color: #7f1d1d; background: #1c0505; color: #fca5a5;
|
||||
padding: 3px 12px; font-size: 11px;
|
||||
}
|
||||
.estop-btn:hover { background: #3b0606; }
|
||||
.estop-btn.active {
|
||||
background: #7f1d1d; border-color: var(--red); color: #fff;
|
||||
animation: blink .8s infinite;
|
||||
}
|
||||
|
||||
.resume-btn {
|
||||
border-color: #14532d; background: #052010; color: #86efac;
|
||||
padding: 3px 12px; font-size: 11px;
|
||||
}
|
||||
.resume-btn:hover { background: #0a3a1a; }
|
||||
|
||||
/* ── Toolbar ── */
|
||||
#toolbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
padding: 5px 12px;
|
||||
background: var(--bg1);
|
||||
border-bottom: 1px solid var(--bd);
|
||||
flex-shrink: 0;
|
||||
flex-wrap: wrap;
|
||||
font-size: 10px;
|
||||
}
|
||||
.tsep { width: 1px; height: 16px; background: var(--bd2); }
|
||||
.tlbl { color: var(--mid); letter-spacing: .08em; }
|
||||
.tval { color: #67e8f9; min-width: 70px; font-family: monospace; }
|
||||
|
||||
.tslider {
|
||||
-webkit-appearance: none;
|
||||
width: 100px; height: 4px; border-radius: 2px;
|
||||
background: var(--bd2); outline: none; cursor: pointer;
|
||||
}
|
||||
.tslider::-webkit-slider-thumb {
|
||||
-webkit-appearance: none;
|
||||
width: 12px; height: 12px; border-radius: 50%;
|
||||
background: var(--cyan); cursor: pointer;
|
||||
}
|
||||
.tslider::-moz-range-thumb {
|
||||
width: 12px; height: 12px; border-radius: 50%;
|
||||
background: var(--cyan); cursor: pointer; border: none;
|
||||
}
|
||||
|
||||
/* ── Main ── */
|
||||
#main {
|
||||
flex: 1;
|
||||
display: grid;
|
||||
grid-template-columns: 1fr 220px;
|
||||
min-height: 0;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
/* ── Joystick area ── */
|
||||
#joystick-area {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-evenly;
|
||||
gap: 12px;
|
||||
padding: 16px;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.stick-wrap {
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
flex-shrink: 0;
|
||||
}
|
||||
.stick-label {
|
||||
font-size: 9px; letter-spacing: .12em; color: var(--mid); text-transform: uppercase;
|
||||
}
|
||||
.stick-vals {
|
||||
font-size: 10px; color: #67e8f9; font-family: monospace; min-width: 100px; text-align: center;
|
||||
}
|
||||
|
||||
canvas {
|
||||
display: block;
|
||||
border-radius: 50%;
|
||||
border: 1px solid var(--bd2);
|
||||
touch-action: none;
|
||||
cursor: grab;
|
||||
}
|
||||
canvas.active { cursor: grabbing; }
|
||||
|
||||
/* ── Center panel ── */
|
||||
#center-panel {
|
||||
flex: 1;
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
gap: 8px;
|
||||
max-width: 260px;
|
||||
position: relative;
|
||||
}
|
||||
|
||||
.cp-block {
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 6px;
|
||||
padding: 8px;
|
||||
}
|
||||
.cp-title {
|
||||
font-size: 9px; font-weight: bold; letter-spacing: .12em;
|
||||
color: #0891b2; text-transform: uppercase; margin-bottom: 5px;
|
||||
}
|
||||
.cp-row {
|
||||
display: flex; justify-content: space-between;
|
||||
padding: 2px 0; border-bottom: 1px solid var(--bd);
|
||||
font-size: 10px;
|
||||
}
|
||||
.cp-row:last-child { border-bottom: none; }
|
||||
.cp-lbl { color: var(--mid); }
|
||||
.cp-val { color: var(--hi); font-family: monospace; }
|
||||
|
||||
/* Keyboard diagram */
|
||||
.key-grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, 28px);
|
||||
grid-template-rows: repeat(3, 22px);
|
||||
gap: 3px;
|
||||
justify-content: center;
|
||||
margin: 4px 0;
|
||||
}
|
||||
.key {
|
||||
background: var(--bg1);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 3px;
|
||||
display: flex; align-items: center; justify-content: center;
|
||||
font-size: 9px; font-weight: bold; color: var(--mid);
|
||||
transition: background .1s, color .1s, border-color .1s;
|
||||
}
|
||||
.key.pressed {
|
||||
background: #0e4f69; border-color: var(--cyan); color: #fff;
|
||||
}
|
||||
.key-wide {
|
||||
grid-column: 1 / 4;
|
||||
width: 100%; height: 22px;
|
||||
}
|
||||
|
||||
/* E-stop overlay */
|
||||
#estop-panel {
|
||||
position: absolute; inset: 0;
|
||||
background: rgba(127,0,0,0.9);
|
||||
border: 2px solid var(--red);
|
||||
border-radius: 8px;
|
||||
display: flex; flex-direction: column;
|
||||
align-items: center; justify-content: center;
|
||||
gap: 4px;
|
||||
animation: blink .8s infinite;
|
||||
}
|
||||
.estop-text { font-size: 28px; font-weight: bold; color: #fca5a5; letter-spacing: .1em; }
|
||||
.estop-sub { font-size: 11px; color: #fca5a5; letter-spacing: .2em; }
|
||||
|
||||
/* ── Sidebar ── */
|
||||
#sidebar {
|
||||
background: var(--bg1);
|
||||
border-left: 1px solid var(--bd);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
overflow-y: auto;
|
||||
padding: 8px;
|
||||
gap: 8px;
|
||||
font-size: 11px;
|
||||
}
|
||||
.sb-card {
|
||||
background: var(--bg2);
|
||||
border: 1px solid var(--bd2);
|
||||
border-radius: 6px;
|
||||
padding: 8px;
|
||||
}
|
||||
.sb-title {
|
||||
font-size: 9px; font-weight: bold; letter-spacing: .12em;
|
||||
color: #0891b2; text-transform: uppercase; margin-bottom: 6px;
|
||||
}
|
||||
.sb-row {
|
||||
display: flex; justify-content: space-between;
|
||||
padding: 2px 0; border-bottom: 1px solid var(--bd);
|
||||
font-size: 10px;
|
||||
}
|
||||
.sb-row:last-child { border-bottom: none; }
|
||||
.sb-lbl { color: var(--mid); }
|
||||
.sb-val { color: var(--hi); font-family: monospace; }
|
||||
|
||||
/* Gamepad axis bars */
|
||||
.gp-axis-row {
|
||||
display: flex; align-items: center; gap: 4px;
|
||||
font-size: 9px; color: var(--mid); margin-bottom: 3px;
|
||||
}
|
||||
.gp-axis-label { width: 30px; flex-shrink: 0; }
|
||||
.gp-bar-track {
|
||||
flex: 1; height: 6px; background: var(--bg0);
|
||||
border: 1px solid var(--bd); border-radius: 3px; overflow: hidden;
|
||||
position: relative;
|
||||
}
|
||||
.gp-bar-center {
|
||||
position: absolute; top: 0; bottom: 0;
|
||||
left: 50%; width: 1px; background: var(--dim);
|
||||
}
|
||||
.gp-bar-fill {
|
||||
position: absolute; top: 0; bottom: 0;
|
||||
background: var(--cyan); transition: none;
|
||||
}
|
||||
.gp-axis-val { width: 32px; text-align: right; color: var(--hi); flex-shrink: 0; }
|
||||
|
||||
.gp-btn-row {
|
||||
display: flex; flex-wrap: wrap; gap: 3px;
|
||||
margin-top: 6px;
|
||||
}
|
||||
.gp-btn-chip {
|
||||
font-size: 8px; padding: 1px 5px; border-radius: 2px;
|
||||
border: 1px solid var(--bd); background: var(--bg0); color: var(--dim);
|
||||
transition: background .1s, color .1s;
|
||||
}
|
||||
.gp-btn-chip.pressed { background: var(--bd2); color: var(--hi); border-color: var(--cyan); }
|
||||
|
||||
/* ── Footer ── */
|
||||
#footer {
|
||||
background: var(--bg1); border-top: 1px solid var(--bd);
|
||||
padding: 3px 12px;
|
||||
display: flex; align-items: center; justify-content: space-between;
|
||||
flex-shrink: 0; font-size: 10px; color: var(--dim);
|
||||
}
|
||||
|
||||
/* ── Responsive ── */
|
||||
@media (max-width: 800px) {
|
||||
#main { grid-template-columns: 1fr; }
|
||||
#sidebar { display: none; }
|
||||
}
|
||||
@media (max-width: 560px) {
|
||||
#right-wrap { display: none; }
|
||||
#center-panel { max-width: 100%; }
|
||||
.tslider { width: 70px; }
|
||||
}
|
||||
197
ui/gamepad_panel.html
Normal file
197
ui/gamepad_panel.html
Normal file
@ -0,0 +1,197 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0, maximum-scale=1.0">
|
||||
<title>Saltybot — Gamepad Teleop</title>
|
||||
<link rel="stylesheet" href="gamepad_panel.css">
|
||||
<script src="https://cdn.jsdelivr.net/npm/roslib@1.3.0/build/roslib.min.js"></script>
|
||||
</head>
|
||||
<body>
|
||||
|
||||
<!-- ── Header ── -->
|
||||
<div id="header">
|
||||
<div class="logo">⚡ SALTYBOT — GAMEPAD TELEOP</div>
|
||||
<div id="conn-dot"></div>
|
||||
<input id="ws-input" type="text" value="ws://localhost:9090" placeholder="ws://robot-ip:9090" />
|
||||
<button id="btn-connect" class="hbtn">CONNECT</button>
|
||||
<span id="conn-label" style="color:#4b5563;font-size:10px">Not connected</span>
|
||||
<div style="flex:1"></div>
|
||||
<div id="gp-indicator">
|
||||
<div id="gp-dot"></div>
|
||||
<span id="gp-label">No gamepad</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- ── Toolbar ── -->
|
||||
<div id="toolbar">
|
||||
<!-- Speed limiter -->
|
||||
<span class="tlbl">LINEAR</span>
|
||||
<input id="slider-linear" type="range" min="10" max="100" value="50" class="tslider">
|
||||
<span id="val-linear" class="tval">0.50 m/s</span>
|
||||
|
||||
<div class="tsep"></div>
|
||||
|
||||
<span class="tlbl">ANGULAR</span>
|
||||
<input id="slider-angular" type="range" min="10" max="100" value="50" class="tslider">
|
||||
<span id="val-angular" class="tval">1.00 rad/s</span>
|
||||
|
||||
<div class="tsep"></div>
|
||||
|
||||
<span class="tlbl">DEADZONE</span>
|
||||
<input id="slider-dz" type="range" min="0" max="40" value="10" class="tslider" style="width:70px">
|
||||
<span id="val-dz" class="tval">10%</span>
|
||||
|
||||
<div class="tsep"></div>
|
||||
|
||||
<button id="btn-estop" class="hbtn estop-btn">⛔ E-STOP</button>
|
||||
<button id="btn-resume" class="hbtn resume-btn" style="display:none">▶ RESUME</button>
|
||||
</div>
|
||||
|
||||
<!-- ── Main ── -->
|
||||
<div id="main">
|
||||
|
||||
<!-- Virtual joystick area -->
|
||||
<div id="joystick-area">
|
||||
<div class="stick-wrap" id="left-wrap">
|
||||
<div class="stick-label">LEFT — DRIVE</div>
|
||||
<canvas id="left-stick" width="200" height="200"></canvas>
|
||||
<div class="stick-vals" id="left-vals">↕ 0.00 m/s</div>
|
||||
</div>
|
||||
|
||||
<!-- Center info -->
|
||||
<div id="center-panel">
|
||||
<div class="cp-block">
|
||||
<div class="cp-title">COMMAND</div>
|
||||
<div class="cp-row">
|
||||
<span class="cp-lbl">Linear</span>
|
||||
<span class="cp-val" id="disp-linear">0.00 m/s</span>
|
||||
</div>
|
||||
<div class="cp-row">
|
||||
<span class="cp-lbl">Angular</span>
|
||||
<span class="cp-val" id="disp-angular">0.00 rad/s</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="cp-block">
|
||||
<div class="cp-title">LIMITS</div>
|
||||
<div class="cp-row">
|
||||
<span class="cp-lbl">Max lin</span>
|
||||
<span class="cp-val" id="lim-linear">0.50 m/s</span>
|
||||
</div>
|
||||
<div class="cp-row">
|
||||
<span class="cp-lbl">Max ang</span>
|
||||
<span class="cp-val" id="lim-angular">1.00 rad/s</span>
|
||||
</div>
|
||||
<div class="cp-row">
|
||||
<span class="cp-lbl">Deadzone</span>
|
||||
<span class="cp-val" id="lim-dz">10%</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="cp-block">
|
||||
<div class="cp-title">KEYBOARD</div>
|
||||
<div class="key-grid">
|
||||
<div></div>
|
||||
<div class="key" id="key-w">W</div>
|
||||
<div></div>
|
||||
<div class="key" id="key-a">A</div>
|
||||
<div class="key" id="key-s">S</div>
|
||||
<div class="key" id="key-d">D</div>
|
||||
<div></div>
|
||||
<div class="key key-wide" id="key-space">SPC</div>
|
||||
<div></div>
|
||||
</div>
|
||||
<div style="font-size:9px;color:#4b5563;text-align:center;margin-top:4px">
|
||||
W/S=fwd/rev · A/D=turn · SPC=stop
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Gamepad layout diagram -->
|
||||
<div class="cp-block">
|
||||
<div class="cp-title">GAMEPAD MAPPING</div>
|
||||
<div style="font-size:9px;color:#6b7280;line-height:1.8">
|
||||
<div>L-stick ↕ → linear vel</div>
|
||||
<div>R-stick ↔ → angular vel</div>
|
||||
<div>LT/RT → fine speed ctrl</div>
|
||||
<div>B/Circle → E-stop toggle</div>
|
||||
<div>Start → Resume</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="estop-panel" style="display:none">
|
||||
<div class="estop-text">⛔ E-STOP</div>
|
||||
<div class="estop-sub">ACTIVE</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="stick-wrap" id="right-wrap">
|
||||
<div class="stick-label">RIGHT — STEER</div>
|
||||
<canvas id="right-stick" width="200" height="200"></canvas>
|
||||
<div class="stick-vals" id="right-vals">↔ 0.00 rad/s</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Sidebar -->
|
||||
<aside id="sidebar">
|
||||
|
||||
<!-- Status -->
|
||||
<div class="sb-card">
|
||||
<div class="sb-title">Status</div>
|
||||
<div class="sb-row">
|
||||
<span class="sb-lbl">E-stop</span>
|
||||
<span class="sb-val" id="sb-estop" style="color:#6b7280">OFF</span>
|
||||
</div>
|
||||
<div class="sb-row">
|
||||
<span class="sb-lbl">Input</span>
|
||||
<span class="sb-val" id="sb-input">—</span>
|
||||
</div>
|
||||
<div class="sb-row">
|
||||
<span class="sb-lbl">Pub rate</span>
|
||||
<span class="sb-val" id="sb-rate">—</span>
|
||||
</div>
|
||||
<div class="sb-row">
|
||||
<span class="sb-lbl">Topic</span>
|
||||
<span class="sb-val" style="font-size:9px">/cmd_vel</span>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Gamepad raw -->
|
||||
<div class="sb-card">
|
||||
<div class="sb-title">Gamepad Raw</div>
|
||||
<div id="gp-raw">
|
||||
<div style="color:#374151;font-size:10px;text-align:center;padding:12px 0">
|
||||
No gamepad connected.<br>Press any button to activate.
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Topics -->
|
||||
<div class="sb-card">
|
||||
<div class="sb-title">Topics</div>
|
||||
<div style="font-size:9px;color:#374151;line-height:1.9">
|
||||
<div>PUB <code style="color:#4b5563">/cmd_vel</code></div>
|
||||
<div style="color:#1e3a5f;padding-left:8px">geometry_msgs/Twist</div>
|
||||
<div style="margin-top:4px;color:#6b7280">20 Hz publish rate</div>
|
||||
<div style="margin-top:4px;color:#6b7280">Sends zero on E-stop</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
</aside>
|
||||
</div>
|
||||
|
||||
<!-- ── Footer ── -->
|
||||
<div id="footer">
|
||||
<span>virtual sticks · WASD keyboard · Web Gamepad API</span>
|
||||
<span>rosbridge: <code id="footer-ws">ws://localhost:9090</code></span>
|
||||
<span>gamepad teleop — issue #598</span>
|
||||
</div>
|
||||
|
||||
<script src="gamepad_panel.js"></script>
|
||||
<script>
|
||||
document.getElementById('ws-input').addEventListener('input', (e) => {
|
||||
document.getElementById('footer-ws').textContent = e.target.value;
|
||||
});
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
548
ui/gamepad_panel.js
Normal file
548
ui/gamepad_panel.js
Normal file
@ -0,0 +1,548 @@
|
||||
/* gamepad_panel.js — Saltybot Gamepad Teleop (Issue #598) */
|
||||
'use strict';
|
||||
|
||||
// ── Constants ──────────────────────────────────────────────────────────────
|
||||
const MAX_LINEAR = 1.0; // absolute max m/s
|
||||
const MAX_ANGULAR = 2.0; // absolute max rad/s
|
||||
const PUBLISH_HZ = 20;
|
||||
const PUBLISH_MS = 1000 / PUBLISH_HZ;
|
||||
|
||||
// ── State ──────────────────────────────────────────────────────────────────
|
||||
const state = {
|
||||
connected: false,
|
||||
estop: false,
|
||||
// Speed limits (0..1 fraction of max)
|
||||
linLimit: 0.5,
|
||||
angLimit: 0.5,
|
||||
deadzone: 0.10,
|
||||
// Current command
|
||||
linVel: 0,
|
||||
angVel: 0,
|
||||
// Input source: 'none' | 'keyboard' | 'virtual' | 'gamepad'
|
||||
inputSrc: 'none',
|
||||
// Keyboard keys held
|
||||
keys: { w: false, a: false, s: false, d: false, space: false },
|
||||
// Virtual stick drags
|
||||
sticks: {
|
||||
left: { active: false, dx: 0, dy: 0 },
|
||||
right: { active: false, dx: 0, dy: 0 },
|
||||
},
|
||||
// Gamepad
|
||||
gpIndex: null,
|
||||
gpPrevBtns: [],
|
||||
// Stats
|
||||
pubCount: 0,
|
||||
pubTs: 0,
|
||||
pubRate: 0,
|
||||
};
|
||||
|
||||
// ── ROS ────────────────────────────────────────────────────────────────────
|
||||
let ros = null;
|
||||
let cmdVelTopic = null;
|
||||
|
||||
function rosCmdVelSetup() {
|
||||
cmdVelTopic = new ROSLIB.Topic({
|
||||
ros,
|
||||
name: '/cmd_vel',
|
||||
messageType: 'geometry_msgs/Twist',
|
||||
});
|
||||
}
|
||||
|
||||
function publishTwist(lin, ang) {
|
||||
if (!ros || !cmdVelTopic || !state.connected) return;
|
||||
cmdVelTopic.publish(new ROSLIB.Message({
|
||||
linear: { x: lin, y: 0, z: 0 },
|
||||
angular: { x: 0, y: 0, z: ang },
|
||||
}));
|
||||
state.pubCount++;
|
||||
const now = performance.now();
|
||||
if (state.pubTs) {
|
||||
const dt = (now - state.pubTs) / 1000;
|
||||
state.pubRate = 1 / dt;
|
||||
}
|
||||
state.pubTs = now;
|
||||
}
|
||||
|
||||
function sendStop() { publishTwist(0, 0); }
|
||||
|
||||
// ── Connection ─────────────────────────────────────────────────────────────
|
||||
const $connDot = document.getElementById('conn-dot');
|
||||
const $connLabel = document.getElementById('conn-label');
|
||||
const $btnConn = document.getElementById('btn-connect');
|
||||
const $wsInput = document.getElementById('ws-input');
|
||||
|
||||
function connect() {
|
||||
const url = $wsInput.value.trim() || 'ws://localhost:9090';
|
||||
if (ros) { try { ros.close(); } catch(_){} }
|
||||
|
||||
$connLabel.textContent = 'Connecting…';
|
||||
$connLabel.style.color = '#d97706';
|
||||
$connDot.className = '';
|
||||
|
||||
ros = new ROSLIB.Ros({ url });
|
||||
|
||||
ros.on('connection', () => {
|
||||
state.connected = true;
|
||||
$connDot.className = 'connected';
|
||||
$connLabel.style.color = '#22c55e';
|
||||
$connLabel.textContent = 'Connected';
|
||||
rosCmdVelSetup();
|
||||
localStorage.setItem('gp_ws_url', url);
|
||||
});
|
||||
|
||||
ros.on('error', () => {
|
||||
state.connected = false;
|
||||
$connDot.className = 'error';
|
||||
$connLabel.style.color = '#ef4444';
|
||||
$connLabel.textContent = 'Error';
|
||||
});
|
||||
|
||||
ros.on('close', () => {
|
||||
state.connected = false;
|
||||
$connDot.className = '';
|
||||
$connLabel.style.color = '#6b7280';
|
||||
$connLabel.textContent = 'Disconnected';
|
||||
});
|
||||
}
|
||||
|
||||
$btnConn.addEventListener('click', connect);
|
||||
|
||||
// Restore saved URL
|
||||
const savedUrl = localStorage.getItem('gp_ws_url');
|
||||
if (savedUrl) {
|
||||
$wsInput.value = savedUrl;
|
||||
document.getElementById('footer-ws').textContent = savedUrl;
|
||||
}
|
||||
|
||||
// ── E-stop ─────────────────────────────────────────────────────────────────
|
||||
const $btnEstop = document.getElementById('btn-estop');
|
||||
const $btnResume = document.getElementById('btn-resume');
|
||||
const $estopPanel = document.getElementById('estop-panel');
|
||||
const $sbEstop = document.getElementById('sb-estop');
|
||||
|
||||
function activateEstop() {
|
||||
state.estop = true;
|
||||
sendStop();
|
||||
$btnEstop.style.display = 'none';
|
||||
$btnResume.style.display = '';
|
||||
$btnEstop.classList.add('active');
|
||||
$estopPanel.style.display = 'flex';
|
||||
$sbEstop.textContent = 'ACTIVE';
|
||||
$sbEstop.style.color = '#ef4444';
|
||||
}
|
||||
|
||||
function resumeFromEstop() {
|
||||
state.estop = false;
|
||||
$btnEstop.style.display = '';
|
||||
$btnResume.style.display = 'none';
|
||||
$btnEstop.classList.remove('active');
|
||||
$estopPanel.style.display = 'none';
|
||||
$sbEstop.textContent = 'OFF';
|
||||
$sbEstop.style.color = '#6b7280';
|
||||
}
|
||||
|
||||
$btnEstop.addEventListener('click', activateEstop);
|
||||
$btnResume.addEventListener('click', resumeFromEstop);
|
||||
|
||||
// ── Sliders ────────────────────────────────────────────────────────────────
|
||||
function setupSlider(id, valId, minVal, maxVal, onUpdate) {
|
||||
const slider = document.getElementById(id);
|
||||
const valEl = document.getElementById(valId);
|
||||
slider.addEventListener('input', () => {
|
||||
const frac = parseInt(slider.value) / 100;
|
||||
onUpdate(frac, valEl);
|
||||
});
|
||||
// Init
|
||||
const frac = parseInt(slider.value) / 100;
|
||||
onUpdate(frac, valEl);
|
||||
}
|
||||
|
||||
setupSlider('slider-linear', 'val-linear', 0, MAX_LINEAR, (frac, el) => {
|
||||
state.linLimit = frac;
|
||||
const ms = (frac * MAX_LINEAR).toFixed(2);
|
||||
el.textContent = ms + ' m/s';
|
||||
document.getElementById('lim-linear').textContent = ms + ' m/s';
|
||||
});
|
||||
|
||||
setupSlider('slider-angular', 'val-angular', 0, MAX_ANGULAR, (frac, el) => {
|
||||
state.angLimit = frac;
|
||||
const rs = (frac * MAX_ANGULAR).toFixed(2);
|
||||
el.textContent = rs + ' rad/s';
|
||||
document.getElementById('lim-angular').textContent = rs + ' rad/s';
|
||||
});
|
||||
|
||||
// Deadzone slider (0–40%)
|
||||
const $dzSlider = document.getElementById('slider-dz');
|
||||
const $dzVal = document.getElementById('val-dz');
|
||||
const $limDz = document.getElementById('lim-dz');
|
||||
$dzSlider.addEventListener('input', () => {
|
||||
state.deadzone = parseInt($dzSlider.value) / 100;
|
||||
$dzVal.textContent = parseInt($dzSlider.value) + '%';
|
||||
$limDz.textContent = parseInt($dzSlider.value) + '%';
|
||||
});
|
||||
|
||||
// ── Keyboard input ─────────────────────────────────────────────────────────
|
||||
const KEY_EL = {
|
||||
w: document.getElementById('key-w'),
|
||||
a: document.getElementById('key-a'),
|
||||
s: document.getElementById('key-s'),
|
||||
d: document.getElementById('key-d'),
|
||||
space: document.getElementById('key-space'),
|
||||
};
|
||||
|
||||
function setKeyState(key, down) {
|
||||
if (!(key in state.keys)) return;
|
||||
state.keys[key] = down;
|
||||
if (KEY_EL[key]) KEY_EL[key].classList.toggle('pressed', down);
|
||||
}
|
||||
|
||||
document.addEventListener('keydown', (e) => {
|
||||
if (e.target.tagName === 'INPUT') return;
|
||||
switch (e.code) {
|
||||
case 'KeyW': case 'ArrowUp': setKeyState('w', true); break;
|
||||
case 'KeyS': case 'ArrowDown': setKeyState('s', true); break;
|
||||
case 'KeyA': case 'ArrowLeft': setKeyState('a', true); break;
|
||||
case 'KeyD': case 'ArrowRight': setKeyState('d', true); break;
|
||||
case 'Space':
|
||||
e.preventDefault();
|
||||
if (state.estop) { resumeFromEstop(); } else { activateEstop(); }
|
||||
break;
|
||||
}
|
||||
});
|
||||
|
||||
document.addEventListener('keyup', (e) => {
|
||||
switch (e.code) {
|
||||
case 'KeyW': case 'ArrowUp': setKeyState('w', false); break;
|
||||
case 'KeyS': case 'ArrowDown': setKeyState('s', false); break;
|
||||
case 'KeyA': case 'ArrowLeft': setKeyState('a', false); break;
|
||||
case 'KeyD': case 'ArrowRight': setKeyState('d', false); break;
|
||||
}
|
||||
});
|
||||
|
||||
function getKeyboardCommand() {
|
||||
let lin = 0, ang = 0;
|
||||
if (state.keys.w) lin += 1;
|
||||
if (state.keys.s) lin -= 1;
|
||||
if (state.keys.a) ang += 1;
|
||||
if (state.keys.d) ang -= 1;
|
||||
return { lin, ang };
|
||||
}
|
||||
|
||||
// ── Virtual Joysticks ──────────────────────────────────────────────────────
|
||||
function setupVirtualStick(canvasId, stickKey, onValue) {
|
||||
const canvas = document.getElementById(canvasId);
|
||||
const R = canvas.width / 2;
|
||||
const ctx = canvas.getContext('2d');
|
||||
const stick = state.sticks[stickKey];
|
||||
let pointerId = null;
|
||||
|
||||
function draw() {
|
||||
const W = canvas.width, H = canvas.height;
|
||||
ctx.clearRect(0, 0, W, H);
|
||||
|
||||
// Base circle
|
||||
ctx.beginPath();
|
||||
ctx.arc(R, R, R - 2, 0, Math.PI * 2);
|
||||
ctx.fillStyle = 'rgba(10,10,26,0.9)';
|
||||
ctx.fill();
|
||||
ctx.strokeStyle = '#1e3a5f';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.stroke();
|
||||
|
||||
// Crosshair
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.lineWidth = 0.5;
|
||||
ctx.beginPath(); ctx.moveTo(R, 4); ctx.lineTo(R, H - 4); ctx.stroke();
|
||||
ctx.beginPath(); ctx.moveTo(4, R); ctx.lineTo(W - 4, R); ctx.stroke();
|
||||
|
||||
// Deadzone ring
|
||||
const dzR = state.deadzone * (R - 10);
|
||||
ctx.beginPath();
|
||||
ctx.arc(R, R, dzR, 0, Math.PI * 2);
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.setLineDash([3, 3]);
|
||||
ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
|
||||
// Knob
|
||||
const kx = R + stick.dx * (R - 16);
|
||||
const ky = R + stick.dy * (R - 16);
|
||||
const kColor = stick.active ? '#06b6d4' : '#1e3a5f';
|
||||
ctx.beginPath();
|
||||
ctx.arc(kx, ky, 16, 0, Math.PI * 2);
|
||||
ctx.fillStyle = kColor + '44';
|
||||
ctx.fill();
|
||||
ctx.strokeStyle = kColor;
|
||||
ctx.lineWidth = 2;
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
function getOffset(e) {
|
||||
const rect = canvas.getBoundingClientRect();
|
||||
return {
|
||||
x: (e.clientX - rect.left) / rect.width * canvas.width,
|
||||
y: (e.clientY - rect.top) / rect.height * canvas.height,
|
||||
};
|
||||
}
|
||||
|
||||
function pointerdown(e) {
|
||||
canvas.setPointerCapture(e.pointerId);
|
||||
pointerId = e.pointerId;
|
||||
stick.active = true;
|
||||
canvas.classList.add('active');
|
||||
update(e);
|
||||
}
|
||||
|
||||
function pointermove(e) {
|
||||
if (!stick.active || e.pointerId !== pointerId) return;
|
||||
update(e);
|
||||
}
|
||||
|
||||
function pointerup(e) {
|
||||
if (e.pointerId !== pointerId) return;
|
||||
stick.active = false;
|
||||
stick.dx = 0;
|
||||
stick.dy = 0;
|
||||
pointerId = null;
|
||||
canvas.classList.remove('active');
|
||||
onValue(0, 0);
|
||||
draw();
|
||||
}
|
||||
|
||||
function update(e) {
|
||||
const { x, y } = getOffset(e);
|
||||
let dx = (x - R) / (R - 16);
|
||||
let dy = (y - R) / (R - 16);
|
||||
const dist = Math.sqrt(dx * dx + dy * dy);
|
||||
if (dist > 1) { dx /= dist; dy /= dist; }
|
||||
stick.dx = dx;
|
||||
stick.dy = dy;
|
||||
// Apply deadzone
|
||||
const norm = Math.sqrt(dx * dx + dy * dy);
|
||||
if (norm < state.deadzone) { onValue(0, 0); }
|
||||
else {
|
||||
const scale = (norm - state.deadzone) / (1 - state.deadzone);
|
||||
onValue((-dy / norm) * scale, (-dx / norm) * scale); // up=+lin, left=+ang
|
||||
}
|
||||
draw();
|
||||
}
|
||||
|
||||
canvas.addEventListener('pointerdown', pointerdown);
|
||||
canvas.addEventListener('pointermove', pointermove);
|
||||
canvas.addEventListener('pointerup', pointerup);
|
||||
canvas.addEventListener('pointercancel', pointerup);
|
||||
|
||||
draw();
|
||||
return { draw };
|
||||
}
|
||||
|
||||
let virtLeftLin = 0, virtLeftAng = 0;
|
||||
let virtRightLin = 0, virtRightAng = 0;
|
||||
|
||||
const leftStickDraw = setupVirtualStick('left-stick', 'left', (fwd, _) => { virtLeftLin = fwd; updateSidebarInput(); }).draw;
|
||||
const rightStickDraw = setupVirtualStick('right-stick', 'right', (_, turn) => { virtRightAng = turn; updateSidebarInput(); }).draw;
|
||||
|
||||
// Redraw sticks when deadzone changes (for dz ring)
|
||||
$dzSlider.addEventListener('input', () => {
|
||||
if (leftStickDraw) leftStickDraw();
|
||||
if (rightStickDraw) rightStickDraw();
|
||||
});
|
||||
|
||||
// ── Gamepad API ────────────────────────────────────────────────────────────
|
||||
const $gpDot = document.getElementById('gp-dot');
|
||||
const $gpLabel = document.getElementById('gp-label');
|
||||
const $gpRaw = document.getElementById('gp-raw');
|
||||
|
||||
window.addEventListener('gamepadconnected', (e) => {
|
||||
state.gpIndex = e.gamepad.index;
|
||||
$gpDot.classList.add('active');
|
||||
$gpLabel.textContent = e.gamepad.id.substring(0, 28);
|
||||
updateGpRaw();
|
||||
});
|
||||
|
||||
window.addEventListener('gamepaddisconnected', (e) => {
|
||||
if (e.gamepad.index === state.gpIndex) {
|
||||
state.gpIndex = null;
|
||||
state.gpPrevBtns = [];
|
||||
$gpDot.classList.remove('active');
|
||||
$gpLabel.textContent = 'No gamepad';
|
||||
$gpRaw.innerHTML = '<div style="color:#374151;font-size:10px;text-align:center;padding:12px 0">No gamepad connected.<br>Press any button to activate.</div>';
|
||||
}
|
||||
});
|
||||
|
||||
function applyDeadzone(v, dz) {
|
||||
const sign = v >= 0 ? 1 : -1;
|
||||
const abs = Math.abs(v);
|
||||
if (abs < dz) return 0;
|
||||
return sign * (abs - dz) / (1 - dz);
|
||||
}
|
||||
|
||||
function getGamepadCommand() {
|
||||
if (state.gpIndex === null) return null;
|
||||
const gp = navigator.getGamepads()[state.gpIndex];
|
||||
if (!gp) return null;
|
||||
|
||||
// Standard mapping:
|
||||
// Axes: 0=LX, 1=LY, 2=RX, 3=RY
|
||||
// Buttons: 0=A/X, 1=B/Circle, 6=LT, 7=RT, 9=Start
|
||||
const dz = state.deadzone;
|
||||
const ly = applyDeadzone(gp.axes[1] || 0, dz);
|
||||
const rx = applyDeadzone(gp.axes[2] || 0, dz);
|
||||
|
||||
// LT/RT fine speed override (reduce max by up to 50%)
|
||||
const lt = gp.buttons[6] ? (gp.buttons[6].value || 0) : 0;
|
||||
const rt = gp.buttons[7] ? (gp.buttons[7].value || 0) : 0;
|
||||
const triggerScale = 1 - (lt * 0.5) - (rt * 0.5) + (rt * 0.5); // RT = full, LT = half
|
||||
const speedScale = lt > 0.1 ? (1 - lt * 0.5) : 1.0;
|
||||
|
||||
// B/Circle = e-stop
|
||||
const bBtn = gp.buttons[1] && gp.buttons[1].pressed;
|
||||
const prevBBtn = state.gpPrevBtns[1] || false;
|
||||
if (bBtn && !prevBBtn) {
|
||||
if (!state.estop) activateEstop(); else resumeFromEstop();
|
||||
}
|
||||
|
||||
// Start = resume
|
||||
const startBtn = gp.buttons[9] && gp.buttons[9].pressed;
|
||||
const prevStart = state.gpPrevBtns[9] || false;
|
||||
if (startBtn && !prevStart && state.estop) resumeFromEstop();
|
||||
|
||||
state.gpPrevBtns = gp.buttons.map(b => b.pressed);
|
||||
|
||||
return { lin: -ly * speedScale, ang: -rx };
|
||||
}
|
||||
|
||||
function updateGpRaw() {
|
||||
if (state.gpIndex === null) return;
|
||||
const gp = navigator.getGamepads()[state.gpIndex];
|
||||
if (!gp) return;
|
||||
|
||||
const AXIS_NAMES = ['LX', 'LY', 'RX', 'RY'];
|
||||
const BTN_NAMES = ['A','B','X','Y','LB','RB','LT','RT','Back','Start',
|
||||
'LS','RS','↑','↓','←','→','Home'];
|
||||
|
||||
let html = '';
|
||||
// Axes
|
||||
for (let i = 0; i < Math.min(gp.axes.length, 4); i++) {
|
||||
const v = gp.axes[i];
|
||||
const pct = ((v + 1) / 2 * 100).toFixed(0);
|
||||
const fill = v >= 0
|
||||
? `left:50%;width:${(v/2*100).toFixed(0)}%`
|
||||
: `left:${((v+1)/2*100).toFixed(0)}%;width:${(Math.abs(v)/2*100).toFixed(0)}%`;
|
||||
html += `<div class="gp-axis-row">
|
||||
<span class="gp-axis-label">${AXIS_NAMES[i]||'A'+i}</span>
|
||||
<div class="gp-bar-track">
|
||||
<div class="gp-bar-center"></div>
|
||||
<div class="gp-bar-fill" style="${fill}"></div>
|
||||
</div>
|
||||
<span class="gp-axis-val">${v.toFixed(2)}</span>
|
||||
</div>`;
|
||||
}
|
||||
// Buttons
|
||||
html += '<div class="gp-btn-row">';
|
||||
for (let i = 0; i < gp.buttons.length; i++) {
|
||||
const pressed = gp.buttons[i].pressed;
|
||||
const name = BTN_NAMES[i] || 'B'+i;
|
||||
html += `<span class="gp-btn-chip${pressed?' pressed':''}">${name}</span>`;
|
||||
}
|
||||
html += '</div>';
|
||||
$gpRaw.innerHTML = html;
|
||||
}
|
||||
|
||||
// ── Velocity display helpers ───────────────────────────────────────────────
|
||||
const $dispLinear = document.getElementById('disp-linear');
|
||||
const $dispAngular = document.getElementById('disp-angular');
|
||||
const $leftVals = document.getElementById('left-vals');
|
||||
const $rightVals = document.getElementById('right-vals');
|
||||
const $sbInput = document.getElementById('sb-input');
|
||||
const $sbRate = document.getElementById('sb-rate');
|
||||
|
||||
function updateSidebarInput() {
|
||||
const src = state.inputSrc;
|
||||
$sbInput.textContent = src === 'gamepad' ? 'Gamepad' :
|
||||
src === 'keyboard' ? 'Keyboard' :
|
||||
src === 'virtual' ? 'Virtual sticks' : '—';
|
||||
}
|
||||
|
||||
// ── Main publish loop ──────────────────────────────────────────────────────
|
||||
let lastPublish = 0;
|
||||
|
||||
function loop(ts) {
|
||||
requestAnimationFrame(loop);
|
||||
|
||||
// Gamepad raw display (60fps)
|
||||
if (state.gpIndex !== null) updateGpRaw();
|
||||
|
||||
// Publish at limited rate
|
||||
if (ts - lastPublish < PUBLISH_MS) return;
|
||||
lastPublish = ts;
|
||||
|
||||
// Determine command from highest-priority active input
|
||||
let lin = 0, ang = 0;
|
||||
let src = 'none';
|
||||
|
||||
// 1. Gamepad (highest priority if connected)
|
||||
const gpCmd = getGamepadCommand();
|
||||
if (gpCmd && (Math.abs(gpCmd.lin) > 0.001 || Math.abs(gpCmd.ang) > 0.001)) {
|
||||
lin = gpCmd.lin;
|
||||
ang = gpCmd.ang;
|
||||
src = 'gamepad';
|
||||
}
|
||||
// 2. Keyboard
|
||||
else {
|
||||
const kbCmd = getKeyboardCommand();
|
||||
if (kbCmd.lin !== 0 || kbCmd.ang !== 0) {
|
||||
lin = kbCmd.lin;
|
||||
ang = kbCmd.ang;
|
||||
src = 'keyboard';
|
||||
}
|
||||
// 3. Virtual sticks
|
||||
else if (state.sticks.left.active || state.sticks.right.active) {
|
||||
lin = virtLeftLin;
|
||||
ang = virtRightAng;
|
||||
src = 'virtual';
|
||||
}
|
||||
}
|
||||
|
||||
// Apply speed limits
|
||||
lin = lin * state.linLimit * MAX_LINEAR;
|
||||
ang = ang * state.angLimit * MAX_ANGULAR;
|
||||
|
||||
// Clamp
|
||||
lin = Math.max(-MAX_LINEAR, Math.min(MAX_LINEAR, lin));
|
||||
ang = Math.max(-MAX_ANGULAR, Math.min(MAX_ANGULAR, ang));
|
||||
|
||||
state.linVel = lin;
|
||||
state.angVel = ang;
|
||||
state.inputSrc = src;
|
||||
|
||||
if (state.estop) { lin = 0; ang = 0; }
|
||||
|
||||
// Publish
|
||||
publishTwist(lin, ang);
|
||||
|
||||
// Update displays
|
||||
$dispLinear.textContent = lin.toFixed(2) + ' m/s';
|
||||
$dispAngular.textContent = ang.toFixed(2) + ' rad/s';
|
||||
$leftVals.textContent = '↕ ' + lin.toFixed(2) + ' m/s';
|
||||
$rightVals.textContent = '↔ ' + ang.toFixed(2) + ' rad/s';
|
||||
|
||||
updateSidebarInput();
|
||||
$sbRate.textContent = state.pubRate > 0 ? state.pubRate.toFixed(1) + ' Hz' : '—';
|
||||
|
||||
// Color command display
|
||||
const linColor = Math.abs(lin) > 0.01 ? '#22c55e' : '#6b7280';
|
||||
const angColor = Math.abs(ang) > 0.01 ? '#06b6d4' : '#6b7280';
|
||||
$dispLinear.style.color = linColor;
|
||||
$dispAngular.style.color = angColor;
|
||||
$leftVals.style.color = linColor;
|
||||
$rightVals.style.color = angColor;
|
||||
}
|
||||
|
||||
requestAnimationFrame(loop);
|
||||
|
||||
// ── Init auto-connect if URL saved ────────────────────────────────────────
|
||||
if (savedUrl) {
|
||||
connect();
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user