mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-15 17:12:59 -04:00
first push of
This commit is contained in:
legop3
@@ -11,4 +11,28 @@ on each roomba:
|
||||
pi provisioning:
|
||||
- enable serial port
|
||||
- disable wifi powersave
|
||||
- disable bluetooth
|
||||
- disable bluetooth
|
||||
|
||||
## Repo layout
|
||||
|
||||
- `pi/roverd`: tiny Go daemon that bridges the Create 2 serial port, BRC pin, and the control server via WebSockets.
|
||||
- `server`: Node.js process that terminates rover sockets, relays commands to/from the Socket.IO UI, and serves `public/`.
|
||||
- `pi/systemd` / `pi/mediamtx`: ready-to-drop systemd units and a minimal mediaMTX config for WebRTC publishing (roverd can optionally supervise the mediamtx service).
|
||||
- `docs/pi-deployment.md`: per-rover build + install instructions (cross-compiling on Fedora 43, deploying roverd + mediaMTX).
|
||||
|
||||
## Quick start
|
||||
|
||||
```bash
|
||||
# build the Pi agent (armv7)
|
||||
cd pi/roverd
|
||||
mkdir -p ../../dist
|
||||
make pi-build
|
||||
|
||||
# start the server + UI
|
||||
cd ../../server
|
||||
npm install
|
||||
npm run start
|
||||
```
|
||||
|
||||
Then point each rover's `/etc/roverd.yaml` at `ws://<server>:8080/rover`, enable the sensor stream from the UI, and drive with WASD.
|
||||
Use the “Restart Camera” button if you enable media management so roverd can bounce the mediamtx service remotely.
|
||||
|
||||
@@ -0,0 +1,67 @@
|
||||
# Pi Deployment Guide
|
||||
|
||||
## Tooling
|
||||
|
||||
Fedora 43:
|
||||
|
||||
```bash
|
||||
sudo dnf install golang libgpiod
|
||||
```
|
||||
|
||||
Cross-compiling roverd for Pi Zero 2 W (ARMv7):
|
||||
|
||||
```bash
|
||||
cd pi/roverd
|
||||
mkdir -p ../../dist
|
||||
make pi-build
|
||||
```
|
||||
|
||||
The binary is placed in `dist/roverd` (relative to the repo root).
|
||||
|
||||
## Installing `roverd`
|
||||
|
||||
1. Copy the binary and config:
|
||||
```bash
|
||||
sudo useradd -r -s /usr/sbin/nologin roverd || true
|
||||
sudo install -o roverd -g roverd -m 0755 dist/roverd /usr/local/bin/roverd
|
||||
sudo install -o roverd -g roverd -m 0640 pi/roverd/roverd.sample.yaml /etc/roverd.yaml
|
||||
```
|
||||
Adjust `/etc/roverd.yaml` for each rover: `name`, `serverUrl` (e.g. `ws://control-server:8080/rover`), serial port path, battery thresholds, GPIO pin for BRC, and the media WHEP URL that points at the central distribution server.
|
||||
|
||||
2. Install the systemd unit:
|
||||
```bash
|
||||
sudo install -m 0644 pi/systemd/roverd.service /etc/systemd/system/roverd.service
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable --now roverd.service
|
||||
```
|
||||
|
||||
`roverd` requires access to `/dev/ttyAMA0` and `/sys/class/gpio`; keeping it under its own user ensures the rest of the system stays isolated.
|
||||
If you set `media.manage: true` in `/etc/roverd.yaml`, make sure the `roverd` service account can invoke `systemctl <action> <media.service>` (either run the unit as root or grant sudo privileges for that command).
|
||||
|
||||
## Configuring mediaMTX
|
||||
|
||||
1. Download the latest mediaMTX release for ARMv7 and place the binary at `/usr/local/bin/mediamtx`.
|
||||
2. Copy the provided config:
|
||||
```bash
|
||||
sudo useradd -r -s /usr/sbin/nologin mediamtx || true
|
||||
sudo install -d -o mediamtx -g mediamtx /etc/mediamtx
|
||||
sudo install -o mediamtx -g mediamtx -m 0644 pi/mediamtx/mediamtx.yml /etc/mediamtx/mediamtx.yml
|
||||
sudo install -m 0644 pi/systemd/mediamtx.service /etc/systemd/system/mediamtx.service
|
||||
sudo systemctl daemon-reload
|
||||
sudo systemctl enable --now mediamtx.service
|
||||
```
|
||||
|
||||
The sample config uses the Raspberry Pi camera module as the source and exposes a WHEP endpoint at `http://<pi-host>:8889/whep/rovercam`. Point `media.whepUrl` in `roverd.yaml` at this URL so the central server can list it.
|
||||
Expose the mediaMTX HTTP API locally (default `http://127.0.0.1:9997`) and set `media.healthUrl` so `roverd` can monitor the pipeline; `media.service` should match the systemd unit name (default `mediamtx.service`).
|
||||
|
||||
## Server + UI
|
||||
|
||||
From the repo root:
|
||||
|
||||
```bash
|
||||
cd server
|
||||
npm install
|
||||
npm run start
|
||||
```
|
||||
|
||||
This launches the HTTP server (serving the barebones UI) and the rover WebSocket endpoint at `ws://<server>:8080/rover`. The UI expects `roverd` instances to send `hello` frames so it can populate the rover list. Use the mode buttons to emit Start/Safe/Full/Passive/Dock commands (they send the raw OI opcode bytes), tap the sensor toggle to request Group 100 streaming, and use WASD for drive testing; the UI emits Drive Direct commands ~8 times per second, so the rover sees them immediately.
|
||||
@@ -0,0 +1,16 @@
|
||||
logLevel: info
|
||||
webrtc: yes
|
||||
webrtcLocalTCPAddress: 0.0.0.0
|
||||
webrtcLocalUDPAddress: 0.0.0.0
|
||||
webrtcDisableDefaultICEServers: true
|
||||
webrtcICEServers:
|
||||
- urls:
|
||||
- stun:stun.l.google.com:19302
|
||||
paths:
|
||||
rovercam:
|
||||
source: rpiCamera
|
||||
rpiCameraWidth: 1280
|
||||
rpiCameraHeight: 720
|
||||
rpiCameraFrameRate: 30
|
||||
rpiCameraHFlip: false
|
||||
rpiCameraVFlip: false
|
||||
@@ -0,0 +1,15 @@
|
||||
BIN_DIR ?= ../../dist
|
||||
GOOS ?= linux
|
||||
GOARCH ?= arm
|
||||
GOARM ?= 6
|
||||
|
||||
.PHONY: build pi-build clean
|
||||
|
||||
build:
|
||||
go build -o $(BIN_DIR)/roverd ./cmd/roverd
|
||||
|
||||
pi-build:
|
||||
GOOS=$(GOOS) GOARCH=$(GOARCH) GOARM=$(GOARM) go build -trimpath -ldflags="-s -w" -o $(BIN_DIR)/roverd ./cmd/roverd
|
||||
|
||||
clean:
|
||||
rm -f $(BIN_DIR)/roverd
|
||||
@@ -0,0 +1,83 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"fmt"
|
||||
"log"
|
||||
"os"
|
||||
"path/filepath"
|
||||
"strconv"
|
||||
"time"
|
||||
)
|
||||
|
||||
type BRCPulser struct {
|
||||
cfg BRCConfig
|
||||
logger *log.Logger
|
||||
}
|
||||
|
||||
func NewBRCPulser(cfg BRCConfig, logger *log.Logger) (*BRCPulser, error) {
|
||||
if err := exportGPIO(cfg.GPIOPin); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
if err := writeGPIO(cfg.GPIOPin, "direction", []byte("out\n")); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
if err := writeGPIO(cfg.GPIOPin, "value", []byte("1\n")); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return &BRCPulser{cfg: cfg, logger: logger}, nil
|
||||
}
|
||||
|
||||
func (b *BRCPulser) Close() {
|
||||
_ = writeGPIO(b.cfg.GPIOPin, "value", []byte("1\n"))
|
||||
_ = unexportGPIO(b.cfg.GPIOPin)
|
||||
}
|
||||
|
||||
func (b *BRCPulser) Start(ctx context.Context) {
|
||||
go func() {
|
||||
ticker := time.NewTicker(b.cfg.PulseEvery.Duration)
|
||||
defer ticker.Stop()
|
||||
|
||||
for {
|
||||
b.pulseOnce()
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (b *BRCPulser) pulseOnce() {
|
||||
if err := writeGPIO(b.cfg.GPIOPin, "value", []byte("0\n")); err != nil {
|
||||
b.logger.Printf("brc pulse low: %v", err)
|
||||
return
|
||||
}
|
||||
time.Sleep(b.cfg.PulseWidth.Duration)
|
||||
if err := writeGPIO(b.cfg.GPIOPin, "value", []byte("1\n")); err != nil {
|
||||
b.logger.Printf("brc pulse high: %v", err)
|
||||
}
|
||||
}
|
||||
|
||||
func exportGPIO(pin int) error {
|
||||
err := os.WriteFile("/sys/class/gpio/export", []byte(strconv.Itoa(pin)), 0o644)
|
||||
if err != nil && !os.IsExist(err) {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func unexportGPIO(pin int) error {
|
||||
err := os.WriteFile("/sys/class/gpio/unexport", []byte(strconv.Itoa(pin)), 0o644)
|
||||
if err != nil && !os.IsNotExist(err) {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func writeGPIO(pin int, field string, data []byte) error {
|
||||
path := filepath.Join("/sys/class/gpio", fmt.Sprintf("gpio%d", pin), field)
|
||||
return os.WriteFile(path, data, 0o644)
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"context"
|
||||
"flag"
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"syscall"
|
||||
"time"
|
||||
|
||||
roverd "multiroombarover/pi/roverd"
|
||||
)
|
||||
|
||||
func main() {
|
||||
var cfgPath string
|
||||
flag.StringVar(&cfgPath, "config", "/etc/roverd.yaml", "path to roverd configuration file")
|
||||
flag.Parse()
|
||||
|
||||
cfg, err := roverd.LoadConfig(cfgPath)
|
||||
if err != nil {
|
||||
log.Fatalf("load config: %v", err)
|
||||
}
|
||||
|
||||
ctx, cancel := signal.NotifyContext(context.Background(), syscall.SIGINT, syscall.SIGTERM)
|
||||
defer cancel()
|
||||
|
||||
logger := log.New(os.Stdout, "roverd: ", log.LstdFlags|log.Lmicroseconds|log.LUTC)
|
||||
|
||||
serialPort, err := roverd.OpenSerial(cfg.Serial)
|
||||
if err != nil {
|
||||
logger.Fatalf("open serial: %v", err)
|
||||
}
|
||||
defer serialPort.Close()
|
||||
|
||||
var pulser *roverd.BRCPulser
|
||||
if cfg.BRC.Enabled() {
|
||||
pulser, err = roverd.NewBRCPulser(cfg.BRC, logger)
|
||||
if err != nil {
|
||||
logger.Fatalf("init BRC pulser: %v", err)
|
||||
}
|
||||
defer pulser.Close()
|
||||
pulser.Start(ctx)
|
||||
}
|
||||
|
||||
sensorFrames := make(chan []byte, 8)
|
||||
streamer := roverd.NewSensorStreamer(serialPort, sensorFrames, logger)
|
||||
go streamer.Run(ctx)
|
||||
|
||||
adapter := roverd.NewSerialAdapter(serialPort, logger)
|
||||
|
||||
mediaSupervisor := roverd.NewMediaSupervisor(cfg.Media, logger)
|
||||
if mediaSupervisor != nil {
|
||||
mediaSupervisor.Start(ctx)
|
||||
}
|
||||
|
||||
client := roverd.NewWSClient(cfg, adapter, sensorFrames, mediaSupervisor, logger)
|
||||
|
||||
retryDelay := time.Second
|
||||
for ctx.Err() == nil {
|
||||
if err := client.Run(ctx); err != nil {
|
||||
logger.Printf("websocket loop ended: %v", err)
|
||||
}
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-time.After(retryDelay):
|
||||
}
|
||||
|
||||
if retryDelay < 30*time.Second {
|
||||
retryDelay *= 2
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
package roverd
|
||||
|
||||
type helloMessage struct {
|
||||
Type string `json:"type"`
|
||||
Name string `json:"name"`
|
||||
Battery BatteryConfig `json:"battery"`
|
||||
MaxWheelSpeed int `json:"maxWheelSpeed"`
|
||||
Media MediaConfig `json:"media"`
|
||||
}
|
||||
|
||||
type sensorMessage struct {
|
||||
Type string `json:"type"`
|
||||
Timestamp int64 `json:"ts"`
|
||||
Data string `json:"data"`
|
||||
}
|
||||
|
||||
type inboundMessage struct {
|
||||
Type string `json:"type"`
|
||||
ID string `json:"id"`
|
||||
DriveDirect *driveDirectPayload `json:"driveDirect,omitempty"`
|
||||
MotorPWM *motorPWMPayload `json:"motorPwm,omitempty"`
|
||||
Raw string `json:"raw,omitempty"`
|
||||
SensorStream *sensorStreamPayload `json:"sensorStream,omitempty"`
|
||||
Media *mediaCommand `json:"media,omitempty"`
|
||||
}
|
||||
|
||||
type driveDirectPayload struct {
|
||||
Left int `json:"left"`
|
||||
Right int `json:"right"`
|
||||
}
|
||||
|
||||
type motorPWMPayload struct {
|
||||
Main int `json:"main"`
|
||||
Side int `json:"side"`
|
||||
Vacuum int `json:"vacuum"`
|
||||
}
|
||||
|
||||
type sensorStreamPayload struct {
|
||||
Enable bool `json:"enable"`
|
||||
}
|
||||
|
||||
type mediaCommand struct {
|
||||
Action string `json:"action"`
|
||||
}
|
||||
|
||||
type ackMessage struct {
|
||||
Type string `json:"type"`
|
||||
ID string `json:"id"`
|
||||
Status string `json:"status"`
|
||||
Error string `json:"error,omitempty"`
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"fmt"
|
||||
"os"
|
||||
"time"
|
||||
|
||||
"gopkg.in/yaml.v3"
|
||||
)
|
||||
|
||||
type SerialConfig struct {
|
||||
Device string `yaml:"device"`
|
||||
Baud int `yaml:"baud"`
|
||||
}
|
||||
|
||||
type Duration struct {
|
||||
time.Duration
|
||||
}
|
||||
|
||||
func (d *Duration) UnmarshalYAML(value *yaml.Node) error {
|
||||
var raw string
|
||||
if err := value.Decode(&raw); err != nil {
|
||||
return err
|
||||
}
|
||||
parsed, err := time.ParseDuration(raw)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
d.Duration = parsed
|
||||
return nil
|
||||
}
|
||||
|
||||
func (d Duration) MarshalYAML() (interface{}, error) {
|
||||
return d.Duration.String(), nil
|
||||
}
|
||||
|
||||
type BRCConfig struct {
|
||||
GPIOPin int `yaml:"gpioPin"`
|
||||
PulseEvery Duration `yaml:"pulseEvery"`
|
||||
PulseWidth Duration `yaml:"pulseWidth"`
|
||||
}
|
||||
|
||||
func (b BRCConfig) Enabled() bool {
|
||||
return b.GPIOPin >= 0
|
||||
}
|
||||
|
||||
type BatteryConfig struct {
|
||||
Full int `yaml:"full"`
|
||||
Warn int `yaml:"warn"`
|
||||
Urgent int `yaml:"urgent"`
|
||||
}
|
||||
|
||||
type MediaConfig struct {
|
||||
WhepURL string `yaml:"whepUrl"`
|
||||
StreamKey string `yaml:"streamKey"`
|
||||
Manage bool `yaml:"manage"`
|
||||
Service string `yaml:"service"`
|
||||
HealthURL string `yaml:"healthUrl"`
|
||||
HealthInterval Duration `yaml:"healthInterval"`
|
||||
}
|
||||
|
||||
type Config struct {
|
||||
Name string `yaml:"name"`
|
||||
ServerURL string `yaml:"serverUrl"`
|
||||
Serial SerialConfig `yaml:"serial"`
|
||||
BRC BRCConfig `yaml:"brc"`
|
||||
Battery BatteryConfig `yaml:"battery"`
|
||||
MaxWheelMMs int `yaml:"maxWheelSpeed"`
|
||||
Media MediaConfig `yaml:"media"`
|
||||
}
|
||||
|
||||
func LoadConfig(path string) (*Config, error) {
|
||||
data, err := os.ReadFile(path)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
cfg := Config{
|
||||
MaxWheelMMs: 500,
|
||||
BRC: BRCConfig{
|
||||
GPIOPin: -1,
|
||||
PulseEvery: Duration{
|
||||
Duration: time.Minute,
|
||||
},
|
||||
PulseWidth: Duration{
|
||||
Duration: time.Second,
|
||||
},
|
||||
},
|
||||
Media: MediaConfig{
|
||||
HealthInterval: Duration{Duration: 30 * time.Second},
|
||||
},
|
||||
}
|
||||
if err := yaml.Unmarshal(data, &cfg); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
if cfg.Name == "" {
|
||||
return nil, errors.New("missing name")
|
||||
}
|
||||
if cfg.ServerURL == "" {
|
||||
return nil, errors.New("missing serverUrl")
|
||||
}
|
||||
if cfg.Serial.Device == "" || cfg.Serial.Baud == 0 {
|
||||
return nil, errors.New("serial device/baud required")
|
||||
}
|
||||
if cfg.Battery.Full == 0 {
|
||||
return nil, errors.New("battery thresholds required")
|
||||
}
|
||||
if cfg.MaxWheelMMs <= 0 || cfg.MaxWheelMMs > 500 {
|
||||
return nil, fmt.Errorf("maxWheelSpeed must be 1-500, got %d", cfg.MaxWheelMMs)
|
||||
}
|
||||
if cfg.Media.Manage && cfg.Media.Service == "" {
|
||||
return nil, errors.New("media.manage requires media.service")
|
||||
}
|
||||
if cfg.Media.Manage && cfg.Media.HealthInterval.Duration <= 0 {
|
||||
cfg.Media.HealthInterval = Duration{Duration: 30 * time.Second}
|
||||
}
|
||||
return &cfg, nil
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
module multiroombarover/pi/roverd
|
||||
|
||||
go 1.25.4
|
||||
|
||||
require (
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07
|
||||
gopkg.in/yaml.v3 v3.0.1
|
||||
nhooyr.io/websocket v1.8.17
|
||||
)
|
||||
|
||||
require golang.org/x/sys v0.38.0 // indirect
|
||||
@@ -0,0 +1,10 @@
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07 h1:UyzmZLoiDWMRywV4DUYb9Fbt8uiOSooupjTq10vpvnU=
|
||||
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07/go.mod h1:kDXzergiv9cbyO7IOYJZWg1U88JhDg3PB6klq9Hg2pA=
|
||||
golang.org/x/sys v0.38.0 h1:3yZWxaJjBmCWXqhN1qh02AkOnCQ1poK6oF+a7xWL6Gc=
|
||||
golang.org/x/sys v0.38.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks=
|
||||
gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405 h1:yhCVgyC4o1eVCa2tZl7eS0r+SDo693bJlVdllGtEeKM=
|
||||
gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=
|
||||
gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA=
|
||||
gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM=
|
||||
nhooyr.io/websocket v1.8.17 h1:KEVeLJkUywCKVsnLIDlD/5gtayKp8VoCkksHCGGfT9Y=
|
||||
nhooyr.io/websocket v1.8.17/go.mod h1:rN9OFWIUwuxg4fR5tELlYC04bXYowCP9GX47ivo2l+c=
|
||||
@@ -0,0 +1,121 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"net/http"
|
||||
"os/exec"
|
||||
"time"
|
||||
)
|
||||
|
||||
type MediaSupervisor struct {
|
||||
cfg MediaConfig
|
||||
logger *log.Logger
|
||||
client *http.Client
|
||||
checkInterval time.Duration
|
||||
}
|
||||
|
||||
func NewMediaSupervisor(cfg MediaConfig, logger *log.Logger) *MediaSupervisor {
|
||||
if !cfg.Manage || cfg.Service == "" {
|
||||
return nil
|
||||
}
|
||||
interval := cfg.HealthInterval.Duration
|
||||
if interval <= 0 {
|
||||
interval = 30 * time.Second
|
||||
}
|
||||
return &MediaSupervisor{
|
||||
cfg: cfg,
|
||||
logger: logger,
|
||||
client: &http.Client{Timeout: 5 * time.Second},
|
||||
checkInterval: interval,
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) Start(ctx context.Context) {
|
||||
if m == nil || m.cfg.HealthURL == "" {
|
||||
return
|
||||
}
|
||||
go func() {
|
||||
ticker := time.NewTicker(m.checkInterval)
|
||||
defer ticker.Stop()
|
||||
|
||||
if err := m.checkAndRepair(); err != nil {
|
||||
m.logger.Printf("media supervisor: %v", err)
|
||||
}
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case <-ticker.C:
|
||||
if err := m.checkAndRepair(); err != nil {
|
||||
m.logger.Printf("media supervisor: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
}()
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) HandleAction(ctx context.Context, action string) error {
|
||||
if m == nil {
|
||||
return errors.New("media supervisor disabled")
|
||||
}
|
||||
switch action {
|
||||
case "start", "stop", "restart", "reload", "status":
|
||||
return m.runSystemctl(ctx, action)
|
||||
default:
|
||||
return fmt.Errorf("unknown media action: %s", action)
|
||||
}
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) checkAndRepair() error {
|
||||
ctx, cancel := context.WithTimeout(context.Background(), 7*time.Second)
|
||||
defer cancel()
|
||||
|
||||
if m.checkHealth(ctx) {
|
||||
return nil
|
||||
}
|
||||
m.logger.Printf("media supervisor: health check failed, restarting %s", m.cfg.Service)
|
||||
if err := m.runSystemctl(ctx, "restart"); err != nil {
|
||||
return fmt.Errorf("restart mediamtx: %w", err)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) checkHealth(ctx context.Context) bool {
|
||||
req, err := http.NewRequestWithContext(ctx, http.MethodGet, m.cfg.HealthURL, nil)
|
||||
if err != nil {
|
||||
m.logger.Printf("media supervisor: health request: %v", err)
|
||||
return false
|
||||
}
|
||||
resp, err := m.client.Do(req)
|
||||
if err != nil {
|
||||
m.logger.Printf("media supervisor: health request failed: %v", err)
|
||||
return false
|
||||
}
|
||||
defer resp.Body.Close()
|
||||
_, _ = io.Copy(io.Discard, resp.Body)
|
||||
if resp.StatusCode >= 200 && resp.StatusCode < 300 {
|
||||
return true
|
||||
}
|
||||
m.logger.Printf("media supervisor: unexpected health status %d", resp.StatusCode)
|
||||
return false
|
||||
}
|
||||
|
||||
func (m *MediaSupervisor) runSystemctl(ctx context.Context, action string) error {
|
||||
if m.cfg.Service == "" {
|
||||
return errors.New("no media service configured")
|
||||
}
|
||||
runCtx, cancel := context.WithTimeout(ctx, 15*time.Second)
|
||||
defer cancel()
|
||||
|
||||
cmd := exec.CommandContext(runCtx, "systemctl", action, m.cfg.Service)
|
||||
output, err := cmd.CombinedOutput()
|
||||
if err != nil {
|
||||
return fmt.Errorf("systemctl %s %s: %w (%s)", action, m.cfg.Service, err, string(output))
|
||||
}
|
||||
return nil
|
||||
}
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1,22 @@
|
||||
# Sample configuration for roverd
|
||||
name: roomba-alpha
|
||||
serverUrl: ws://control-server.local:8080/rover
|
||||
serial:
|
||||
device: /dev/ttyAMA0
|
||||
baud: 115200
|
||||
brc:
|
||||
gpioPin: 17
|
||||
pulseEvery: 1m
|
||||
pulseWidth: 1s
|
||||
battery:
|
||||
full: 2068
|
||||
warn: 1700
|
||||
urgent: 1650
|
||||
maxWheelSpeed: 350
|
||||
media:
|
||||
whepUrl: https://mediaserver.local/whep/roomba-alpha
|
||||
streamKey: roomba-alpha
|
||||
manage: false
|
||||
service: mediamtx.service
|
||||
healthUrl: http://127.0.0.1:9997/v3/paths/list/rovercam
|
||||
healthInterval: 30s
|
||||
@@ -0,0 +1,22 @@
|
||||
# Sample configuration for roverd
|
||||
name: roomba-alpha
|
||||
serverUrl: ws://control-server.local:8080/rover
|
||||
serial:
|
||||
device: /dev/ttyAMA0
|
||||
baud: 115200
|
||||
brc:
|
||||
gpioPin: 17
|
||||
pulseEvery: 1m
|
||||
pulseWidth: 1s
|
||||
battery:
|
||||
full: 2068
|
||||
warn: 1700
|
||||
urgent: 1650
|
||||
maxWheelSpeed: 350
|
||||
media:
|
||||
whepUrl: https://mediaserver.local/whep/roomba-alpha
|
||||
streamKey: roomba-alpha
|
||||
manage: false
|
||||
service: mediamtx.service
|
||||
healthUrl: http://127.0.0.1:9997/v3/paths/list/rovercam
|
||||
healthInterval: 30s
|
||||
@@ -0,0 +1,76 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"context"
|
||||
"encoding/hex"
|
||||
"io"
|
||||
"log"
|
||||
"time"
|
||||
)
|
||||
|
||||
const (
|
||||
sensorHeader = 19
|
||||
sensorReadTimeout = 150 * time.Millisecond
|
||||
streamGroupDefault = 100
|
||||
)
|
||||
|
||||
type SensorStreamer struct {
|
||||
r io.Reader
|
||||
out chan<- []byte
|
||||
logger *log.Logger
|
||||
}
|
||||
|
||||
func NewSensorStreamer(r io.Reader, out chan<- []byte, logger *log.Logger) *SensorStreamer {
|
||||
return &SensorStreamer{r: r, out: out, logger: logger}
|
||||
}
|
||||
|
||||
func (s *SensorStreamer) Run(ctx context.Context) {
|
||||
reader := bufio.NewReader(s.r)
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
|
||||
b, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
if ctx.Err() != nil {
|
||||
return
|
||||
}
|
||||
continue
|
||||
}
|
||||
if b != sensorHeader {
|
||||
continue
|
||||
}
|
||||
nBytes, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
continue
|
||||
}
|
||||
frame := make([]byte, int(nBytes)+3)
|
||||
frame[0] = sensorHeader
|
||||
frame[1] = nBytes
|
||||
if _, err := io.ReadFull(reader, frame[2:]); err != nil {
|
||||
continue
|
||||
}
|
||||
|
||||
if !validateChecksum(frame) {
|
||||
s.logger.Printf("sensor checksum failed: %s", hex.EncodeToString(frame))
|
||||
continue
|
||||
}
|
||||
|
||||
select {
|
||||
case s.out <- frame:
|
||||
default:
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func validateChecksum(buf []byte) bool {
|
||||
var sum int
|
||||
for _, b := range buf {
|
||||
sum += int(b)
|
||||
}
|
||||
return byte(sum&0xFF) == 0
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"encoding/base64"
|
||||
"fmt"
|
||||
"io"
|
||||
"log"
|
||||
"sync"
|
||||
|
||||
"github.com/tarm/serial"
|
||||
)
|
||||
|
||||
type SerialAdapter struct {
|
||||
port io.ReadWriteCloser
|
||||
encoder *base64.Encoding
|
||||
mu sync.Mutex
|
||||
log *log.Logger
|
||||
}
|
||||
|
||||
func OpenSerial(cfg SerialConfig) (*serial.Port, error) {
|
||||
return serial.OpenPort(&serial.Config{
|
||||
Name: cfg.Device,
|
||||
Baud: cfg.Baud,
|
||||
ReadTimeout: sensorReadTimeout,
|
||||
})
|
||||
}
|
||||
|
||||
func NewSerialAdapter(port io.ReadWriteCloser, logger *log.Logger) *SerialAdapter {
|
||||
return &SerialAdapter{
|
||||
port: port,
|
||||
encoder: base64.StdEncoding,
|
||||
log: logger,
|
||||
}
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) write(buf []byte) error {
|
||||
s.mu.Lock()
|
||||
defer s.mu.Unlock()
|
||||
|
||||
n, err := s.port.Write(buf)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
if n != len(buf) {
|
||||
return fmt.Errorf("short write %d/%d", n, len(buf))
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) DriveDirect(left, right int) error {
|
||||
payload := []byte{
|
||||
145,
|
||||
byte((right >> 8) & 0xFF),
|
||||
byte(right & 0xFF),
|
||||
byte((left >> 8) & 0xFF),
|
||||
byte(left & 0xFF),
|
||||
}
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) MotorPWM(main, side, vacuum int) error {
|
||||
payload := []byte{
|
||||
144,
|
||||
byte(main & 0xFF),
|
||||
byte(side & 0xFF),
|
||||
byte(vacuum & 0xFF),
|
||||
}
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) StartSensorStream(group byte) error {
|
||||
payload := []byte{148, 1, group}
|
||||
return s.write(payload)
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) PauseSensorStream(pause bool) error {
|
||||
state := byte(1)
|
||||
if pause {
|
||||
state = 0
|
||||
}
|
||||
return s.write([]byte{150, state})
|
||||
}
|
||||
|
||||
func (s *SerialAdapter) SendRaw(raw []byte) error {
|
||||
return s.write(raw)
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
package roverd
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/base64"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"log"
|
||||
"time"
|
||||
|
||||
"nhooyr.io/websocket"
|
||||
)
|
||||
|
||||
type WSClient struct {
|
||||
cfg *Config
|
||||
adapter *SerialAdapter
|
||||
sensorFrames <-chan []byte
|
||||
media *MediaSupervisor
|
||||
log *log.Logger
|
||||
}
|
||||
|
||||
func NewWSClient(cfg *Config, adapter *SerialAdapter, frames <-chan []byte, media *MediaSupervisor, logger *log.Logger) *WSClient {
|
||||
return &WSClient{
|
||||
cfg: cfg,
|
||||
adapter: adapter,
|
||||
sensorFrames: frames,
|
||||
media: media,
|
||||
log: logger,
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) Run(ctx context.Context) error {
|
||||
conn, _, err := websocket.Dial(ctx, c.cfg.ServerURL, nil)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer conn.Close(websocket.StatusInternalError, "closed")
|
||||
|
||||
if err := c.sendHello(ctx, conn); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
errCh := make(chan error, 1)
|
||||
go func() {
|
||||
errCh <- c.readLoop(ctx, conn)
|
||||
}()
|
||||
go c.forwardSensors(ctx, conn)
|
||||
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
conn.Close(websocket.StatusNormalClosure, "context done")
|
||||
return ctx.Err()
|
||||
case err := <-errCh:
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) sendHello(ctx context.Context, conn *websocket.Conn) error {
|
||||
msg := helloMessage{
|
||||
Type: "hello",
|
||||
Name: c.cfg.Name,
|
||||
Battery: c.cfg.Battery,
|
||||
MaxWheelSpeed: c.cfg.MaxWheelMMs,
|
||||
Media: c.cfg.Media,
|
||||
}
|
||||
return writeJSON(ctx, conn, msg)
|
||||
}
|
||||
|
||||
func (c *WSClient) readLoop(ctx context.Context, conn *websocket.Conn) error {
|
||||
for {
|
||||
_, data, err := conn.Read(ctx)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
var msg inboundMessage
|
||||
if err := json.Unmarshal(data, &msg); err != nil {
|
||||
c.log.Printf("invalid command: %v", err)
|
||||
continue
|
||||
}
|
||||
if msg.ID == "" {
|
||||
continue
|
||||
}
|
||||
status := "ok"
|
||||
cmdErr := c.dispatch(ctx, &msg)
|
||||
if cmdErr != nil {
|
||||
status = "error"
|
||||
}
|
||||
ack := ackMessage{
|
||||
Type: "ack",
|
||||
ID: msg.ID,
|
||||
Status: status,
|
||||
}
|
||||
if cmdErr != nil {
|
||||
ack.Error = cmdErr.Error()
|
||||
}
|
||||
if err := writeJSON(ctx, conn, ack); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) dispatch(ctx context.Context, msg *inboundMessage) error {
|
||||
switch {
|
||||
case msg.DriveDirect != nil:
|
||||
left := clamp(msg.DriveDirect.Left, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs)
|
||||
right := clamp(msg.DriveDirect.Right, -c.cfg.MaxWheelMMs, c.cfg.MaxWheelMMs)
|
||||
return c.adapter.DriveDirect(left, right)
|
||||
case msg.MotorPWM != nil:
|
||||
main := clamp(msg.MotorPWM.Main, -127, 127)
|
||||
side := clamp(msg.MotorPWM.Side, -127, 127)
|
||||
vac := clamp(msg.MotorPWM.Vacuum, 0, 127)
|
||||
return c.adapter.MotorPWM(main, side, vac)
|
||||
case msg.SensorStream != nil:
|
||||
if msg.SensorStream.Enable {
|
||||
if err := c.adapter.StartSensorStream(streamGroupDefault); err != nil {
|
||||
return err
|
||||
}
|
||||
return c.adapter.PauseSensorStream(false)
|
||||
}
|
||||
return c.adapter.PauseSensorStream(true)
|
||||
case msg.Raw != "" && len(msg.Raw) > 0:
|
||||
buf, err := base64.StdEncoding.DecodeString(msg.Raw)
|
||||
if err != nil {
|
||||
return fmt.Errorf("raw decode: %w", err)
|
||||
}
|
||||
return c.adapter.SendRaw(buf)
|
||||
case msg.Media != nil:
|
||||
if c.media == nil {
|
||||
return fmt.Errorf("media supervisor disabled")
|
||||
}
|
||||
return c.media.HandleAction(ctx, msg.Media.Action)
|
||||
default:
|
||||
return fmt.Errorf("unsupported command type: %s", msg.Type)
|
||||
}
|
||||
}
|
||||
|
||||
func (c *WSClient) forwardSensors(ctx context.Context, conn *websocket.Conn) {
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
case frame := <-c.sensorFrames:
|
||||
msg := sensorMessage{
|
||||
Type: "sensor",
|
||||
Timestamp: time.Now().UnixMilli(),
|
||||
Data: base64.StdEncoding.EncodeToString(frame),
|
||||
}
|
||||
if err := writeJSON(ctx, conn, msg); err != nil {
|
||||
c.log.Printf("sensor send failed: %v", err)
|
||||
return
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func writeJSON(ctx context.Context, conn *websocket.Conn, v any) error {
|
||||
data, err := json.Marshal(v)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
return conn.Write(ctx, websocket.MessageText, data)
|
||||
}
|
||||
|
||||
func clamp(value, min, max int) int {
|
||||
if value < min {
|
||||
return min
|
||||
}
|
||||
if value > max {
|
||||
return max
|
||||
}
|
||||
return value
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
[Unit]
|
||||
Description=mediaMTX WebRTC publisher
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/usr/local/bin/mediamtx /etc/mediamtx/mediamtx.yml
|
||||
Restart=on-failure
|
||||
RestartSec=5
|
||||
User=mediamtx
|
||||
Group=mediamtx
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
@@ -0,0 +1,16 @@
|
||||
[Unit]
|
||||
Description=Multi-Roomba rover control agent
|
||||
After=network-online.target mediamtx.service
|
||||
Wants=network-online.target
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/usr/local/bin/roverd -config /etc/roverd.yaml
|
||||
Restart=on-failure
|
||||
RestartSec=5
|
||||
User=roverd
|
||||
Group=roverd
|
||||
AmbientCapabilities=CAP_SYS_TTY_CONFIG CAP_SYS_RAWIO
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
Generated
+1560
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"name": "multiroombarover-server",
|
||||
"version": "0.1.0",
|
||||
"private": true,
|
||||
"scripts": {
|
||||
"start": "node src/index.js",
|
||||
"dev": "nodemon src/index.js"
|
||||
},
|
||||
"dependencies": {
|
||||
"express": "^4.19.2",
|
||||
"morgan": "^1.10.0",
|
||||
"socket.io": "^4.7.5",
|
||||
"uuid": "^9.0.1",
|
||||
"ws": "^8.18.0"
|
||||
},
|
||||
"devDependencies": {
|
||||
"nodemon": "^3.1.0"
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
const socket = io();
|
||||
const statusEl = document.getElementById('status');
|
||||
const roverSelect = document.getElementById('roverSelect');
|
||||
const sensorToggleBtn = document.getElementById('sensorToggle');
|
||||
const motorsStopBtn = document.getElementById('motorsStop');
|
||||
const sensorOutput = document.getElementById('sensorOutput');
|
||||
const mediaRestartBtn = document.getElementById('mediaRestart');
|
||||
let selectedRover = null;
|
||||
let sensorEnabled = false;
|
||||
let lastDrive = { left: 0, right: 0 };
|
||||
|
||||
const OI_COMMANDS = {
|
||||
start: [128],
|
||||
safe: [131],
|
||||
full: [132],
|
||||
passive: [128],
|
||||
dock: [143],
|
||||
};
|
||||
|
||||
socket.on('connect', () => {
|
||||
statusEl.textContent = 'Connected';
|
||||
});
|
||||
|
||||
socket.on('disconnect', () => {
|
||||
statusEl.textContent = 'Disconnected';
|
||||
});
|
||||
|
||||
socket.on('rovers', (list) => {
|
||||
roverSelect.innerHTML = '';
|
||||
list.forEach((rover) => {
|
||||
const option = document.createElement('option');
|
||||
option.value = rover.id;
|
||||
option.textContent = `${rover.name} (${rover.id})`;
|
||||
roverSelect.appendChild(option);
|
||||
});
|
||||
if (list.length && !selectedRover) {
|
||||
roverSelect.selectedIndex = 0;
|
||||
selectedRover = list[0].id;
|
||||
}
|
||||
});
|
||||
|
||||
socket.on('sensorFrame', ({ roverId, frame }) => {
|
||||
if (roverId !== selectedRover) return;
|
||||
sensorOutput.textContent = formatSensorFrame(frame.data);
|
||||
});
|
||||
|
||||
socket.on('commandAck', ({ roverId, status, error }) => {
|
||||
if (roverId !== selectedRover) return;
|
||||
if (status === 'ok') {
|
||||
statusEl.textContent = 'Command applied';
|
||||
} else {
|
||||
statusEl.textContent = `Command failed: ${error}`;
|
||||
}
|
||||
});
|
||||
|
||||
roverSelect.addEventListener('change', (e) => {
|
||||
selectedRover = e.target.value;
|
||||
});
|
||||
|
||||
sensorToggleBtn.addEventListener('click', () => {
|
||||
if (!selectedRover) return;
|
||||
sensorEnabled = !sensorEnabled;
|
||||
sensorToggleBtn.textContent = sensorEnabled ? 'Disable Sensor Stream' : 'Enable Sensor Stream';
|
||||
sendCommand('sensorStream', { enable: sensorEnabled });
|
||||
});
|
||||
|
||||
motorsStopBtn.addEventListener('click', () => {
|
||||
sendCommand('motors', { main: 0, side: 0, vacuum: 0 });
|
||||
});
|
||||
|
||||
mediaRestartBtn.addEventListener('click', () => {
|
||||
sendCommand('media', { action: 'restart' });
|
||||
});
|
||||
|
||||
document.querySelectorAll('[data-mode]').forEach((btn) => {
|
||||
btn.addEventListener('click', () => {
|
||||
const mode = btn.dataset.mode;
|
||||
if (!OI_COMMANDS[mode]) return;
|
||||
sendCommand('raw', { bytes: OI_COMMANDS[mode] });
|
||||
});
|
||||
});
|
||||
|
||||
const keys = new Set();
|
||||
const driveInterval = 120;
|
||||
|
||||
window.addEventListener('keydown', (event) => {
|
||||
keys.add(event.key.toLowerCase());
|
||||
});
|
||||
|
||||
window.addEventListener('keyup', (event) => {
|
||||
keys.delete(event.key.toLowerCase());
|
||||
});
|
||||
|
||||
setInterval(() => {
|
||||
if (!selectedRover) return;
|
||||
const speeds = computeDrive();
|
||||
if (speeds.left === lastDrive.left && speeds.right === lastDrive.right) {
|
||||
return;
|
||||
}
|
||||
lastDrive = speeds;
|
||||
sendCommand('drive', speeds);
|
||||
}, driveInterval);
|
||||
|
||||
function computeDrive() {
|
||||
const forward = keys.has('w');
|
||||
const backward = keys.has('s');
|
||||
const left = keys.has('a');
|
||||
const right = keys.has('d');
|
||||
const fast = keys.has('shift');
|
||||
const base = fast ? 300 : 150;
|
||||
let leftSpeed = 0;
|
||||
let rightSpeed = 0;
|
||||
|
||||
if (forward && !backward) {
|
||||
leftSpeed += base;
|
||||
rightSpeed += base;
|
||||
} else if (backward && !forward) {
|
||||
leftSpeed -= base;
|
||||
rightSpeed -= base;
|
||||
}
|
||||
|
||||
if (left && !right) {
|
||||
leftSpeed -= base;
|
||||
rightSpeed += base;
|
||||
} else if (right && !left) {
|
||||
leftSpeed += base;
|
||||
rightSpeed -= base;
|
||||
}
|
||||
|
||||
if (!forward && !backward && (left || right)) {
|
||||
leftSpeed = left ? -base : base;
|
||||
rightSpeed = left ? base : -base;
|
||||
}
|
||||
|
||||
return {
|
||||
left: clamp(leftSpeed, -500, 500),
|
||||
right: clamp(rightSpeed, -500, 500),
|
||||
};
|
||||
}
|
||||
|
||||
function sendCommand(type, data = {}) {
|
||||
if (!selectedRover) return;
|
||||
socket.emit('command', { roverId: selectedRover, type, data });
|
||||
}
|
||||
|
||||
function formatSensorFrame(base64) {
|
||||
const buffer = Uint8Array.from(atob(base64), (c) => c.charCodeAt(0));
|
||||
return Array.from(buffer)
|
||||
.map((b) => b.toString(16).padStart(2, '0'))
|
||||
.join(' ');
|
||||
}
|
||||
|
||||
function clamp(value, min, max) {
|
||||
return Math.max(min, Math.min(max, value));
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
<!doctype html>
|
||||
<html>
|
||||
<head>
|
||||
<meta charset="utf-8" />
|
||||
<title>Multi Roomba Rover</title>
|
||||
<style>
|
||||
body { font-family: sans-serif; margin: 16px; }
|
||||
#layout { display: flex; gap: 16px; }
|
||||
#roverList { width: 240px; }
|
||||
#sensorOutput { font-family: monospace; height: 320px; overflow: auto; border: 1px solid #999; padding: 8px; }
|
||||
button { margin: 4px; }
|
||||
.row { margin-bottom: 8px; }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<h1>Multi Roomba Rover</h1>
|
||||
<div id="status">Connecting…</div>
|
||||
<div id="layout">
|
||||
<div id="roverList">
|
||||
<label for="roverSelect">Rovers</label>
|
||||
<select id="roverSelect" size="10" style="width: 100%"></select>
|
||||
<div class="row">
|
||||
<button data-mode="start">Start OI</button>
|
||||
<button data-mode="safe">Safe</button>
|
||||
<button data-mode="full">Full</button>
|
||||
<button data-mode="passive">Passive</button>
|
||||
<button data-mode="dock">Dock</button>
|
||||
</div>
|
||||
<div class="row">
|
||||
<button id="sensorToggle">Enable Sensor Stream</button>
|
||||
</div>
|
||||
<div class="row">
|
||||
<button id="motorsStop">Stop Motors</button>
|
||||
<button id="mediaRestart">Restart Camera</button>
|
||||
</div>
|
||||
</div>
|
||||
<div style="flex: 1">
|
||||
<h3>Sensor Frames</h3>
|
||||
<pre id="sensorOutput"></pre>
|
||||
<p>Use WASD keys to drive the selected rover. Shift increases speed.</p>
|
||||
</div>
|
||||
</div>
|
||||
<script src="/socket.io/socket.io.js"></script>
|
||||
<script src="app.js" type="module"></script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,171 @@
|
||||
const http = require('http');
|
||||
const path = require('path');
|
||||
const express = require('express');
|
||||
const morgan = require('morgan');
|
||||
const { Server: SocketIOServer } = require('socket.io');
|
||||
const { WebSocketServer } = require('ws');
|
||||
const { v4: uuidv4 } = require('uuid');
|
||||
|
||||
const PORT = process.env.PORT || 8080;
|
||||
|
||||
const app = express();
|
||||
app.use(morgan('dev'));
|
||||
app.use(express.json());
|
||||
app.use(express.static(path.join(__dirname, '..', 'public')));
|
||||
|
||||
const httpServer = http.createServer(app);
|
||||
const io = new SocketIOServer(httpServer, {
|
||||
cors: { origin: '*' },
|
||||
});
|
||||
|
||||
const roverWSS = new WebSocketServer({ noServer: true });
|
||||
const rovers = new Map();
|
||||
const pendingCommands = new Map();
|
||||
|
||||
httpServer.on('upgrade', (req, socket, head) => {
|
||||
if (req.url.startsWith('/rover')) {
|
||||
roverWSS.handleUpgrade(req, socket, head, (ws) => {
|
||||
roverWSS.emit('connection', ws, req);
|
||||
});
|
||||
} else {
|
||||
socket.destroy();
|
||||
}
|
||||
});
|
||||
|
||||
roverWSS.on('connection', (ws) => {
|
||||
handleRoverConnection(ws);
|
||||
});
|
||||
|
||||
function handleRoverConnection(ws) {
|
||||
let roverId = null;
|
||||
ws.on('message', (raw) => {
|
||||
let msg;
|
||||
try {
|
||||
msg = JSON.parse(raw.toString());
|
||||
} catch (err) {
|
||||
return;
|
||||
}
|
||||
|
||||
switch (msg.type) {
|
||||
case 'hello':
|
||||
roverId = msg.name;
|
||||
rovers.set(roverId, {
|
||||
id: roverId,
|
||||
ws,
|
||||
meta: msg,
|
||||
lastSensor: null,
|
||||
lastSeen: Date.now(),
|
||||
});
|
||||
broadcastRoster();
|
||||
break;
|
||||
case 'sensor':
|
||||
if (!roverId || !rovers.has(roverId)) {
|
||||
return;
|
||||
}
|
||||
rovers.get(roverId).lastSensor = msg;
|
||||
rovers.get(roverId).lastSeen = Date.now();
|
||||
io.emit('sensorFrame', { roverId, frame: msg });
|
||||
break;
|
||||
case 'ack':
|
||||
if (msg.id && pendingCommands.has(msg.id)) {
|
||||
const pending = pendingCommands.get(msg.id);
|
||||
pendingCommands.delete(msg.id);
|
||||
io.emit('commandAck', { roverId: pending.roverId, id: msg.id, status: msg.status, error: msg.error });
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
});
|
||||
|
||||
ws.on('close', () => {
|
||||
if (roverId) {
|
||||
rovers.delete(roverId);
|
||||
broadcastRoster();
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
io.on('connection', (socket) => {
|
||||
socket.emit('rovers', getRoster());
|
||||
|
||||
socket.on('command', (payload = {}, cb = () => {}) => {
|
||||
try {
|
||||
const commandId = routeCommand(payload);
|
||||
cb({ id: commandId });
|
||||
} catch (err) {
|
||||
cb({ error: err.message });
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
function getRoster() {
|
||||
return Array.from(rovers.values()).map(({ id, meta, lastSeen }) => ({
|
||||
id,
|
||||
name: meta?.name ?? id,
|
||||
battery: meta?.battery ?? null,
|
||||
maxWheelSpeed: meta?.maxWheelSpeed ?? null,
|
||||
media: meta?.media ?? null,
|
||||
lastSeen,
|
||||
}));
|
||||
}
|
||||
|
||||
function broadcastRoster() {
|
||||
io.emit('rovers', getRoster());
|
||||
}
|
||||
|
||||
function routeCommand(payload) {
|
||||
const { roverId, type, data } = payload;
|
||||
if (!roverId) {
|
||||
throw new Error('roverId missing');
|
||||
}
|
||||
if (!rovers.has(roverId)) {
|
||||
throw new Error(`rover ${roverId} not connected`);
|
||||
}
|
||||
const rover = rovers.get(roverId);
|
||||
const message = buildCommand(type, data);
|
||||
const id = uuidv4();
|
||||
message.id = id;
|
||||
rover.ws.send(JSON.stringify(message));
|
||||
pendingCommands.set(id, { roverId, issuedAt: Date.now(), type });
|
||||
return id;
|
||||
}
|
||||
|
||||
function buildCommand(type, data = {}) {
|
||||
switch (type) {
|
||||
case 'drive':
|
||||
return { type: 'drive', driveDirect: { left: data.left || 0, right: data.right || 0 } };
|
||||
case 'motors':
|
||||
return {
|
||||
type: 'motors',
|
||||
motorPwm: {
|
||||
main: data.main ?? 0,
|
||||
side: data.side ?? 0,
|
||||
vacuum: data.vacuum ?? 0,
|
||||
},
|
||||
};
|
||||
case 'raw':
|
||||
return { type: 'raw', raw: Buffer.from(data.bytes || []).toString('base64') };
|
||||
case 'sensorStream':
|
||||
return { type: 'sensorStream', sensorStream: { enable: Boolean(data.enable) } };
|
||||
case 'media':
|
||||
if (!data || !data.action) {
|
||||
throw new Error('media action required');
|
||||
}
|
||||
return { type: 'media', media: { action: data.action } };
|
||||
default:
|
||||
throw new Error(`unknown command type: ${type}`);
|
||||
}
|
||||
}
|
||||
|
||||
if (require.main === module) {
|
||||
httpServer.listen(PORT, () => {
|
||||
console.log(`Server listening on :${PORT}`);
|
||||
});
|
||||
}
|
||||
|
||||
module.exports = {
|
||||
httpServer,
|
||||
io,
|
||||
routeCommand,
|
||||
};
|
||||
Reference in New Issue
Block a user