mirror of
https://github.com/legop3/MultiRoombaRover.git
synced 2026-09-16 01:21:20 -04:00
first push of
This commit is contained in:
legop3
@@ -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
|
||||
}
|
||||
Reference in New Issue
Block a user