mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 09:31:20 -04:00
first push of
This commit is contained in:
@@ -12,3 +12,27 @@ pi provisioning:
|
|||||||
- enable serial port
|
- enable serial port
|
||||||
- disable wifi powersave
|
- 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