robocars/robocar-throttle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

[driveMode] Adjust throttle on driveMode events

Browse Source
This commit is contained in:
Cyrille Nofficial
2019-12-27 17:42:10 +01:00
parent 83056fb0f1
commit c1286d2246
8 changed files with 184 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
part/part.go
View File

@ -2,23 +2,26 @@ package part
import (
"github.com/cyrilix/robocar-base/mqttdevice"
"github.com/cyrilix/robocar-base/service"
"github.com/cyrilix/robocar-base/types"
mqtt "github.com/eclipse/paho.mqtt.golang"
"log"
"sync"
"time"
)
type CommandValue struct {
Value float64
Confidence float64
}
type Steering CommandValue
type Throttle CommandValue
func NewPart(pub mqttdevice.Publisher, throttleTopic string, minValue, maxValue float64) *ThrottlePart {
func NewPart(client mqtt.Client, pub mqttdevice.Publisher, throttleTopic, driveModeTopic, rcThrottleTopic string,
minValue, maxValue float64, publishPilotFrequency int) *ThrottlePart {
return &ThrottlePart{
pub: pub,
throttleTopic: throttleTopic,
minThrottle: minValue,
maxThrottle: maxValue,
client: client,
pub: pub,
throttleTopic: throttleTopic,
driveModeTopic: driveModeTopic,
rcThrottleTopic: rcThrottleTopic,
minThrottle: minValue,
maxThrottle: maxValue,
driveMode: types.DriveModeUser,
publishPilotFrequency: publishPilotFrequency,
}
}
@ -28,27 +31,91 @@ type ThrottlePart struct {
pub mqttdevice.Publisher
throttleTopic string
minThrottle, maxThrottle float64
cancel chan interface{}
muDriveMode sync.RWMutex
driveMode types.DriveMode
cancel chan interface{}
publishPilotFrequency int
driveModeTopic, rcThrottleTopic string
}
func (p *ThrottlePart) Start() error {
if err := registerCallbacks(p); err != nil {
log.Printf("unable to rgeister callbacks: %v", err)
return err
}
p.cancel = make(chan interface{})
ticker := time.NewTicker(500 * time.Millisecond)
ticker := time.NewTicker(1 * time.Second / time.Duration(p.publishPilotFrequency))
for {
p.pub.Publish(p.throttleTopic, mqttdevice.NewMqttValue(Throttle{
Value: p.minThrottle,
Confidence: 1.0,
}))
select {
case <-ticker.C:
p.publishPilotValue()
case <-p.cancel:
break
}
}
}
func (p *ThrottlePart) publishPilotValue() {
p.muDriveMode.RLock()
defer p.muDriveMode.RUnlock()
if p.driveMode != types.DriveModePilot {
return
}
p.pub.Publish(p.throttleTopic, mqttdevice.NewMqttValue(types.Throttle{
Value: p.minThrottle,
Confidence: 1.0,
}))
}
func (p *ThrottlePart) Stop() {
close(p.cancel)
service.StopService("throttle", p.client, p.driveModeTopic, p.rcThrottleTopic)
}
func (p *ThrottlePart) onDriveMode(_ mqtt.Client, message mqtt.Message) {
payload := message.Payload()
value := mqttdevice.NewMqttValue(payload)
m, err := value.DriveModeValue()
if err != nil {
log.Printf("invalid drive mode: %v", err)
return
}
p.muDriveMode.Lock()
defer p.muDriveMode.Unlock()
p.driveMode = m
}
func (p *ThrottlePart) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
payload := message.Payload()
value := mqttdevice.NewMqttValue(payload)
val, err := value.Float64Value()
if err != nil {
log.Printf("invalid throttle value from arduino: %v", err)
return
}
p.muDriveMode.RLock()
defer p.muDriveMode.RUnlock()
if p.driveMode == types.DriveModeUser {
p.pub.Publish(p.throttleTopic, mqttdevice.NewMqttValue(types.Throttle{Value: val, Confidence: 1.0}))
}
}
var registerCallbacks = func (p *ThrottlePart) error {
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.rcThrottleTopic, p.onRCThrottle)
if err != nil {
return err
}
return nil
}

69
part/part_test.go
View File

@ -2,35 +2,72 @@ package part
import (
"encoding/json"
"github.com/cyrilix/robocar-base/mqttdevice"
"github.com/cyrilix/robocar-base/testtools"
"github.com/cyrilix/robocar-base/types"
"testing"
"time"
)
func TestDefaultThrottle(t *testing.T){
func TestDefaultThrottle(t *testing.T) {
oldRegister := registerCallbacks
defer func(){
registerCallbacks = oldRegister
}()
registerCallbacks = func(p *ThrottlePart) error {
return nil
}
throttleTopic := "topic/throttle"
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
minValue := 0.56
pub := testtools.NewFakePublisher()
p := NewPart(pub, throttleTopic, minValue, 1.)
p := NewPart(nil, pub, throttleTopic, driveModeTopic, rcThrottleTopic, minValue, 1., 200)
cases := []struct {
driveMode types.DriveMode
rcThrottle float64
expectedThrottle float64
}{
{types.DriveModeUser, 0.3, 0.3},
{types.DriveModePilot, 0.5, minValue},
{types.DriveModePilot, 0.4, minValue},
{types.DriveModeUser, 0.5, 0.5},
{types.DriveModeUser, 0.4, 0.4},
{types.DriveModeUser, 0.6, 0.6},
}
go p.Start()
defer p.Stop()
defer func(){close(p.cancel)}()
time.Sleep(1 * time.Millisecond)
for _, c := range cases {
mqttValue := pub.PublishedEvent(throttleTopic)
var throttle Throttle
err := json.Unmarshal(mqttValue, &throttle)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
p.onDriveMode(nil, testtools.NewFakeMessage(driveModeTopic, mqttdevice.NewMqttValue(c.driveMode)))
p.onRCThrottle(nil, testtools.NewFakeMessage(rcThrottleTopic, mqttdevice.NewMqttValue(c.rcThrottle)))
if throttle.Value != minValue {
t.Errorf("bad throttle value: %v, wants %v", throttle.Value, minValue)
}
if throttle.Confidence != 1. {
t.Errorf("bad throtlle confidence: %v, wants %v", throttle.Confidence, 1.)
time.Sleep(10 * time.Millisecond)
for i := 3; i >= 0; i-- {
mqttValue := pub.PublishedEvent(throttleTopic)
var throttle types.Throttle
err := json.Unmarshal(mqttValue, &throttle)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
if throttle.Value != c.expectedThrottle {
t.Errorf("bad throttle value for mode %v: %v, wants %v", c.driveMode, throttle.Value, c.expectedThrottle)
}
if throttle.Confidence != 1. {
t.Errorf("bad throtlle confidence: %v, wants %v", throttle.Confidence, 1.)
}
time.Sleep(1 * time.Millisecond)
}
}
}