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

feat: adapt throttle from steering

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-09-05 16:44:02 +02:00 committed by Cyrille Nofficial
parent c724c1170a
commit d9b0dbb07a
4 changed files with 317 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cmd/rc-throttle/rc-throttle.go
View File

@ -16,8 +16,9 @@ const (
func main() {
var mqttBroker, username, password, clientId string
var throttleTopic, driveModeTopic, rcThrottleTopic string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
var minThrottle, maxThrottle float64
var publishPilotFrequency int
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
if err != nil {
@ -36,8 +37,12 @@ func main() {
flag.StringVar(&throttleTopic, "mqtt-topic-throttle", os.Getenv("MQTT_TOPIC_THROTTLE"), "Mqtt topic to publish throttle result, use MQTT_TOPIC_THROTTLE if args not set")
flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
flag.StringVar(&rcThrottleTopic, "mqtt-topic-rc-throttle", os.Getenv("MQTT_TOPIC_RC_THROTTLE"), "Mqtt topic that contains RC Throttle value, use MQTT_TOPIC_RC_THROTTLE if args not set")
flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering value, use MQTT_TOPIC_STEERING if args not set")
flag.Float64Var(&minThrottle, "throttle-min", minThrottle, "Minimum throttle value, use THROTTLE_MIN if args not set")
flag.Float64Var(&maxThrottle, "throttle-max", maxThrottle, "Minimum throttle value, use THROTTLE_MAX if args not set")
flag.IntVar(&publishPilotFrequency, "update-pwm-frequency", 2, "Number of throttle event to publish when pilot mode is enabled")
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
flag.Parse()
@ -65,7 +70,7 @@ func main() {
}
defer client.Disconnect(50)
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, float32(minThrottle), float32(maxThrottle), 2)
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
defer p.Stop()
cli.HandleExit(p)

58
pkg/throttle/throttle.go → pkg/throttle/controller.go
View File

@ -6,16 +6,18 @@ import (
mqtt "github.com/eclipse/paho.mqtt.golang"
"go.uber.org/zap"
"google.golang.org/protobuf/proto"
"math"
"sync"
"time"
)
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string, minValue, maxValue float32, publishPilotFrequency int) *Controller {
return &Controller{
client: client,
throttleTopic: throttleTopic,
driveModeTopic: driveModeTopic,
rcThrottleTopic: rcThrottleTopic,
steeringTopic: steeringTopic,
minThrottle: minValue,
maxThrottle: maxValue,
driveMode: events.DriveMode_USER,
@ -32,9 +34,12 @@ type Controller struct {
muDriveMode sync.RWMutex
driveMode events.DriveMode
muSteering sync.RWMutex
steering float32
cancel chan interface{}
publishPilotFrequency int
driveModeTopic, rcThrottleTopic string
driveModeTopic, rcThrottleTopic, steeringTopic string
}
func (c *Controller) Start() error {
@ -50,7 +55,7 @@ func (c *Controller) Start() error {
case <-ticker.C:
c.onPublishPilotValue()
case <-c.cancel:
break
return nil
}
}
}
@ -64,21 +69,34 @@ func (c *Controller) onPublishPilotValue() {
}
throttleMsg := events.ThrottleMessage{
Throttle: c.minThrottle,
Throttle: c.computeThrottle(),
Confidence: 1.0,
}
payload, err := proto.Marshal(&throttleMsg)
if err != nil {
zap.S().Errorf("unable to marshal %T protobuf content: %err", throttleMsg, err)
zap.S().Errorf("unable to marshal %v protobuf content: %v", throttleMsg.String(), err)
return
}
publish(c.client, c.throttleTopic, &payload)
publish(c.client, c.throttleTopic, payload)
}
func (c *Controller) computeThrottle() float32 {
s := c.readSteering()
absSteering := math.Abs(float64(s))
return c.minThrottle + float32(float64(c.maxThrottle-c.minThrottle)*(1-absSteering))
}
func (c *Controller) readSteering() float32 {
c.muSteering.RLock()
defer c.muSteering.RUnlock()
return c.steering
}
func (c *Controller) Stop() {
close(c.cancel)
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic)
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
}
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@ -115,13 +133,26 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
zap.S().Errorf("unable to marshall throttle msg: %v", err)
return
}
publish(c.client, c.throttleTopic, &payloadPatched)
publish(c.client, c.throttleTopic, payloadPatched)
return
}
publish(c.client, c.throttleTopic, &payload)
publish(c.client, c.throttleTopic, payload)
}
}
func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
var steeringMsg events.SteeringMessage
payload := message.Payload()
err := proto.Unmarshal(payload, &steeringMsg)
if err != nil {
zap.S().Errorf("unable to unmarshal steering message, skip value: %v", err)
return
}
c.muSteering.Lock()
defer c.muSteering.Unlock()
c.steering = steeringMsg.GetSteering()
}
var registerCallbacks = func(p *Controller) error {
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
if err != nil {
@ -132,9 +163,14 @@ var registerCallbacks = func(p *Controller) error {
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.steeringTopic, p.onSteering)
if err != nil {
return err
}
return nil
}
var publish = func(client mqtt.Client, topic string, payload *[]byte) {
client.Publish(topic, 0, false, *payload)
var publish = func(client mqtt.Client, topic string, payload []byte) {
client.Publish(topic, 0, false, payload)
}

261
pkg/throttle/controller_test.go Normal file
View File

@ -0,0 +1,261 @@
package throttle
import (
"github.com/cyrilix/robocar-base/testtools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
"sync"
"testing"
"time"
)
func TestDefaultThrottle(t *testing.T) {
oldRegister := registerCallbacks
oldPublish := publish
defer func() {
registerCallbacks = oldRegister
publish = oldPublish
}()
registerCallbacks = func(p *Controller) error {
return nil
}
var muEventsPublished sync.Mutex
eventsPublished := make(map[string][]byte)
publish = func(client mqtt.Client, topic string, payload []byte) {
muEventsPublished.Lock()
defer muEventsPublished.Unlock()
eventsPublished[topic] = payload
}
throttleTopic := "topic/throttle"
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
steeringTopic := "topic/rcThrottle"
minValue := float32(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
cases := []struct {
name string
maxThrottle float32
driveMode events.DriveModeMessage
rcThrottle events.ThrottleMessage
expectedThrottle events.ThrottleMessage
}{
{"test1", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}},
{"test2", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0}},
{"test3", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0}},
{"test4", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
{"test5", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
{"test6", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}},
{"limit max throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
}
go p.Start()
defer func() { close(p.cancel) }()
for _, c := range cases {
t.Run(c.name, func(t *testing.T) {
p.maxThrottle = c.maxThrottle
p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
p.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, &c.rcThrottle))
time.Sleep(10 * time.Millisecond)
for i := 3; i >= 0; i-- {
var msg events.ThrottleMessage
muEventsPublished.Lock()
err := proto.Unmarshal(eventsPublished[throttleTopic], &msg)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
muEventsPublished.Unlock()
if msg.GetThrottle() != c.expectedThrottle.GetThrottle() {
t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode.String(), msg.GetThrottle(), c.expectedThrottle.GetThrottle())
}
if msg.GetConfidence() != 1. {
t.Errorf("bad throtlle confidence: %v, wants %v", msg.GetConfidence(), 1.)
}
time.Sleep(1 * time.Millisecond)
}
})
}
}
func TestController_Start(t *testing.T) {
oldRegister := registerCallbacks
oldPublish := publish
defer func() {
registerCallbacks = oldRegister
publish = oldPublish
}()
registerCallbacks = func(p *Controller) error {
return nil
}
publishPilotFrequency := 10
waitPublish := sync.WaitGroup{}
var muEventsPublished sync.Mutex
eventsPublished := make(map[string][]byte)
publish = func(client mqtt.Client, topic string, payload []byte) {
muEventsPublished.Lock()
defer muEventsPublished.Unlock()
eventsPublished[topic] = payload
waitPublish.Done()
}
throttleTopic := "topic/throttle"
steeringTopic := "topic/steering"
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
type fields struct {
driveMode events.DriveMode
min, max float32
publishPilotFrequency int
}
type msgEvents struct {
driveMode *events.DriveModeMessage
steering *events.SteeringMessage
rcThrottle *events.ThrottleMessage
}
tests := []struct {
name string
fields fields
msgEvents msgEvents
want *events.ThrottleMessage
wantErr bool
}{
{
name: "On user drive mode, throttle from rc",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
},
{
name: "On user drive mode, limit throttle to max allowed value",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.9, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
{
name: "On user drive mode, throttle can be < to min allowed value",
fields: fields{
driveMode: events.DriveMode_USER,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_USER},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.1, Confidence: 1.0},
},
{
name: "On pilot drive mode and straight steering, use max throttle allowed",
fields: fields{
driveMode: events.DriveMode_PILOT,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
steering: &events.SteeringMessage{Steering: 0.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
},
{
name: "On pilot drive mode and on left steering, use min throttle allowed",
fields: fields{
driveMode: events.DriveMode_PILOT,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
steering: &events.SteeringMessage{Steering: -1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
},
{
name: "On pilot drive mode and on right steering, use min throttle allowed",
fields: fields{
driveMode: events.DriveMode_PILOT,
max: 0.8,
min: 0.3,
publishPilotFrequency: publishPilotFrequency,
},
msgEvents: msgEvents{
driveMode: &events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
steering: &events.SteeringMessage{Steering: 1.0, Confidence: 1.0},
rcThrottle: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
},
want: &events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0},
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
c := New(nil,
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
tt.fields.min, tt.fields.max,
tt.fields.publishPilotFrequency,
)
go c.Start()
defer func() { close(c.cancel) }()
time.Sleep(1 * time.Millisecond)
// Publish events and wait generation of new steering message
waitPublish.Add(1)
c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, tt.msgEvents.driveMode))
c.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, tt.msgEvents.rcThrottle))
c.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, tt.msgEvents.steering))
waitPublish.Wait()
var msg events.ThrottleMessage
muEventsPublished.Lock()
err := proto.Unmarshal(eventsPublished[throttleTopic], &msg)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
muEventsPublished.Unlock()
if msg.GetThrottle() != tt.want.GetThrottle() {
t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode.String(), msg.GetThrottle(), tt.want.GetThrottle())
}
})
}
}

89
pkg/throttle/throttle_test.go
View File

@ -1,89 +0,0 @@
package throttle
import (
"github.com/cyrilix/robocar-base/testtools"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"google.golang.org/protobuf/proto"
"sync"
"testing"
"time"
)
func TestDefaultThrottle(t *testing.T) {
oldRegister := registerCallbacks
oldPublish := publish
defer func() {
registerCallbacks = oldRegister
publish = oldPublish
}()
registerCallbacks = func(p *Controller) error {
return nil
}
var muEventsPublished sync.Mutex
eventsPublished := make(map[string][]byte)
publish = func(client mqtt.Client, topic string, payload *[]byte) {
muEventsPublished.Lock()
defer muEventsPublished.Unlock()
eventsPublished[topic] = *payload
}
throttleTopic := "topic/throttle"
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
minValue := float32(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, minValue, 1., 200)
cases := []struct {
name string
maxThrottle float32
driveMode events.DriveModeMessage
rcThrottle events.ThrottleMessage
expectedThrottle events.ThrottleMessage
}{
{"test1", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.3, Confidence: 1.0}},
{"test2", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
{"test3", 1., events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: minValue, Confidence: 1.0}},
{"test4", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.5, Confidence: 1.0}},
{"test5", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
{"test6", 1., events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}},
{"limit max throttle on user mode", 0.4, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.ThrottleMessage{Throttle: 0.6, Confidence: 1.0}, events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0}},
}
go p.Start()
defer func() { close(p.cancel) }()
for _, c := range cases {
t.Run(c.name, func(t *testing.T) {
p.maxThrottle = c.maxThrottle
p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
p.onRCThrottle(nil, testtools.NewFakeMessageFromProtobuf(rcThrottleTopic, &c.rcThrottle))
time.Sleep(10 * time.Millisecond)
for i := 3; i >= 0; i-- {
var msg events.ThrottleMessage
muEventsPublished.Lock()
err := proto.Unmarshal(eventsPublished[throttleTopic], &msg)
if err != nil {
t.Errorf("unable to unmarshall response: %v", err)
t.Fail()
}
muEventsPublished.Unlock()
if msg.GetThrottle() != c.expectedThrottle.GetThrottle() {
t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode, msg.GetThrottle(), c.expectedThrottle.GetThrottle())
}
if msg.GetConfidence() != 1. {
t.Errorf("bad throtlle confidence: %v, wants %v", msg.GetConfidence(), 1.)
}
time.Sleep(1 * time.Millisecond)
}
})
}
}