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

feat: add Processor to compute throttle from external values

Browse Source 
* SpeedZoneProcessor
 * SteeringProcessor
This commit is contained in:
Cyrille Nofficial 2023-06-14 20:07:12 +02:00
parent 5c77538181
commit a7b08ff7b4
6 changed files with 289 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,16 +13,19 @@ import (
const (
DefaultClientId = "robocar-throttle"
DefaultThrottleMin = 0.3
DefaultThrottleMin = 0.1
)
func main() {
var mqttBroker, username, password, clientId string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic string
var minThrottle, maxThrottle float64
var publishPilotFrequency int
var brakeConfig string
var enableBrake bool
var enableSpeedZone bool
var slowZoneThrottle, normalZoneThrottle, fastZoneThrottle float64
var moderateSteering, fullSteering float64
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
if err != nil {
@ -43,6 +46,7 @@ func main() {
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.StringVar(&throttleFeedbackTopic, "mqtt-topic-throttle-feedback", os.Getenv("MQTT_TOPIC_THROTTLE_FEEDBACK"), "Mqtt topic where to publish throttle feedback, use MQTT_TOPIC_THROTTLE_FEEDBACK if args not set")
flag.StringVar(&speedZoneTopic, "mqtt-topic-speed-zone", os.Getenv("MQTT_TOPIC_SPEED_ZONE"), "Mqtt topic where to subscribe speed zone events, use MQTT_TOPIC_SPEED_ZONE 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")
@ -50,6 +54,13 @@ func main() {
flag.BoolVar(&enableBrake, "enable-brake-feature", false, "Enable brake to slow car on throttle changes")
flag.StringVar(&brakeConfig, "brake-configuration", "", "Json file to use to configure brake adaptation when --enable-brake is `true`")
flag.BoolVar(&enableSpeedZone, "enable-speed-zone", false, "Enable speed zone information to estimate throttle")
flag.Float64Var(&slowZoneThrottle, "slow-zone-throttle", 0.11, "Throttle target for slow speed zone")
flag.Float64Var(&normalZoneThrottle, "normal-zone-throttle", 0.12, "Throttle target for normal speed zone")
flag.Float64Var(&fastZoneThrottle, "fast-zone-throttle", 0.13, "Throttle target for fast speed zone")
flag.Float64Var(&moderateSteering, "moderate-steering", 0.3, "Steering above is considered as moderate")
flag.Float64Var(&fullSteering, "full-steering", 0.8, "Steering above is considered as full")
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
flag.Parse()
@ -71,15 +82,22 @@ func main() {
}()
zap.ReplaceGlobals(lgr)
zap.S().Infof("Topic throttle : %s", throttleTopic)
zap.S().Infof("Topic rc-throttle : %s", rcThrottleTopic)
zap.S().Infof("Topic throttle feedback : %s", throttleFeedbackTopic)
zap.S().Infof("Topic steering : %s", steeringTopic)
zap.S().Infof("Topic drive mode : %s", driveModeTopic)
zap.S().Infof("Min throttle : %v", minThrottle)
zap.S().Infof("Max throttle : %v", maxThrottle)
zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency)
zap.S().Infof("Brake enabled : %v", enableBrake)
zap.S().Infof("Topic throttle : %s", throttleTopic)
zap.S().Infof("Topic rc-throttle : %s", rcThrottleTopic)
zap.S().Infof("Topic throttle feedback : %s", throttleFeedbackTopic)
zap.S().Infof("Topic steering : %s", steeringTopic)
zap.S().Infof("Topic drive mode : %s", driveModeTopic)
zap.S().Infof("Topic speed zone : %s", speedZoneTopic)
zap.S().Infof("Min throttle : %v", minThrottle)
zap.S().Infof("Max throttle : %v", maxThrottle)
zap.S().Infof("Publish frequency : %vHz", publishPilotFrequency)
zap.S().Infof("Brake enabled : %v", enableBrake)
zap.S().Infof("SpeedZone enabled : %v", enableSpeedZone)
zap.S().Infof("SpeedZone slow throttle : %v", slowZoneThrottle)
zap.S().Infof("SpeedZone normal throttle : %v", normalZoneThrottle)
zap.S().Infof("SpeedZone fast throttle : %v", fastZoneThrottle)
zap.S().Infof("Steering moderate : %v", moderateSteering)
zap.S().Infof("Steering full : %v", fullSteering)
client, err := cli.Connect(mqttBroker, username, password, clientId)
if err != nil {

49
pkg/throttle/controller.go
View File

@ -12,8 +12,9 @@ import (
"time"
)
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
speedZoneTopic string,
maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{
client: client,
throttleTopic: throttleTopic,
@ -21,10 +22,11 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
rcThrottleTopic: rcThrottleTopic,
steeringTopic: steeringTopic,
throttleFeedbackTopic: throttleFeedbackTopic,
speedZoneTopic: speedZoneTopic,
maxThrottle: maxValue,
driveMode: events.DriveMode_USER,
publishPilotFrequency: publishPilotFrequency,
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
processor: &SteeringProcessor{minThrottle: 0.1, maxThrottle: maxValue},
brakeCtrl: &brake.DisabledController{},
}
for _, o := range opts {
@ -41,23 +43,30 @@ func WithBrakeController(bc brake.Controller) Option {
}
}
func WithThrottleProcessor(p Processor) Option {
return func(c *Controller) {
c.processor = p
}
}
type Controller struct {
client mqtt.Client
throttleTopic string
maxThrottle types.Throttle
steeringProcessor *SteeringProcessor
client mqtt.Client
throttleTopic string
maxThrottle types.Throttle
processor Processor
muDriveMode sync.RWMutex
driveMode events.DriveMode
muSteering sync.RWMutex
steering float32
steering types.Steering
brakeCtrl brake.Controller
cancel chan interface{}
publishPilotFrequency int
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
speedZoneTopic string
}
func (c *Controller) Start() error {
@ -86,7 +95,7 @@ func (c *Controller) onPublishPilotValue() {
return
}
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
throttleFromSteering := c.processor.Process(c.readSteering())
throttleMsg := events.ThrottleMessage{
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
@ -102,7 +111,7 @@ func (c *Controller) onPublishPilotValue() {
}
func (c *Controller) readSteering() float32 {
func (c *Controller) readSteering() types.Steering {
c.muSteering.RLock()
defer c.muSteering.RUnlock()
return c.steering
@ -110,7 +119,8 @@ func (c *Controller) readSteering() float32 {
func (c *Controller) Stop() {
close(c.cancel)
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic, c.throttleFeedbackTopic)
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic,
c.throttleFeedbackTopic, c.speedZoneTopic)
}
func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
@ -174,7 +184,18 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
}
c.muSteering.Lock()
defer c.muSteering.Unlock()
c.steering = steeringMsg.GetSteering()
c.steering = types.Steering(steeringMsg.GetSteering())
}
func (c *Controller) onSpeedZone(_ mqtt.Client, message mqtt.Message) {
var szMsg events.SpeedZoneMessage
payload := message.Payload()
err := proto.Unmarshal(payload, &szMsg)
if err != nil {
zap.S().Errorf("unable to unmarshal speedZone message, skip value: %v", err)
return
}
c.processor.SetSpeedZone(szMsg.GetSpeedZone())
}
var registerCallbacks = func(p *Controller) error {
@ -196,6 +217,10 @@ var registerCallbacks = func(p *Controller) error {
if err != nil {
return err
}
err = service.RegisterCallback(p.client, p.speedZoneTopic, p.onSpeedZone)
if err != nil {
return err
}
return nil
}

8
pkg/throttle/controller_test.go
View File

@ -36,10 +36,9 @@ func TestDefaultThrottle(t *testing.T) {
rcThrottleTopic := "topic/rcThrottle"
steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
speedZoneTopic := "topic/speedZone"
minValue := types.Throttle(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, speedZoneTopic, 1., 200)
cases := []*struct {
name string
@ -117,6 +116,7 @@ func TestController_Start(t *testing.T) {
driveModeTopic := "topic/driveMode"
rcThrottleTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
speedZoneTopic := "topic/speedZone"
type fields struct {
driveMode events.DriveMode
@ -263,7 +263,7 @@ func TestController_Start(t *testing.T) {
t.Run(tt.name, func(t *testing.T) {
c := New(nil,
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
tt.fields.min, tt.fields.max,
speedZoneTopic, tt.fields.max,
tt.fields.publishPilotFrequency,
WithBrakeController(tt.fields.brakeCtl),
)

82
pkg/throttle/processor.go
View File

@ -1,16 +1,96 @@
package throttle
import (
"encoding/json"
"fmt"
"github.com/cyrilix/robocar-protobuf/go/events"
"github.com/cyrilix/robocar-throttle/pkg/types"
"math"
"os"
"sync"
)
type Processor interface {
// Process compute throttle from steering value
Process(steering types.Steering) types.Throttle
SetSpeedZone(sz events.SpeedZone)
}
func NewSteeringProcessor(minThrottle, maxThrottle types.Throttle) *SteeringProcessor {
return &SteeringProcessor{
minThrottle: minThrottle,
maxThrottle: maxThrottle,
}
}
type SteeringProcessor struct {
minThrottle, maxThrottle types.Throttle
}
func (sp *SteeringProcessor) SetSpeedZone(_ events.SpeedZone) {
return
}
// Process compute throttle from steering value
func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
func (sp *SteeringProcessor) Process(steering types.Steering) types.Throttle {
absSteering := math.Abs(float64(steering))
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
}
func NewSpeedZoneProcessor(slowThrottle, normalThrottle, fastThrottle types.Throttle,
moderateSteering, fullSteering float64) *SpeedZoneProcessor {
return &SpeedZoneProcessor{
muSz: sync.Mutex{},
speedZone: events.SpeedZone_UNKNOWN,
slowThrottle: slowThrottle,
normalThrottle: normalThrottle,
fastThrottle: fastThrottle,
moderateSteering: moderateSteering,
fullSteering: fullSteering,
}
}
type SpeedZoneProcessor struct {
muSz sync.Mutex
speedZone events.SpeedZone
slowThrottle, normalThrottle, fastThrottle types.Throttle
moderateSteering, fullSteering float64
}
func (sp *SpeedZoneProcessor) SpeedZone() events.SpeedZone {
sp.muSz.Lock()
defer sp.muSz.Unlock()
return sp.speedZone
}
func (sp *SpeedZoneProcessor) SetSpeedZone(sz events.SpeedZone) {
sp.muSz.Lock()
defer sp.muSz.Unlock()
sp.speedZone = sz
}
// Process compute throttle from steering value
func (sp *SpeedZoneProcessor) Process(steering types.Steering) types.Throttle {
st := math.Abs(float64(steering))
switch sp.SpeedZone() {
case events.SpeedZone_FAST:
if st >= sp.fullSteering {
return sp.slowThrottle
} else if st >= sp.moderateSteering {
return sp.normalThrottle
}
return sp.fastThrottle
case events.SpeedZone_NORMAL:
if st > sp.fullSteering {
return sp.slowThrottle
}
return sp.normalThrottle
case events.SpeedZone_SLOW:
return sp.slowThrottle
}
return sp.slowThrottle
}
func NewCustomSteeringProcessor(cfg *Config) *CustomSteeringProcessor {

137
pkg/throttle/processor_test.go
View File

@ -1,7 +1,11 @@
package throttle
import (
"github.com/cyrilix/robocar-protobuf/go/events"
"github.com/cyrilix/robocar-throttle/pkg/types"
"os"
"path"
"reflect"
"testing"
)
@ -11,7 +15,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
maxThrottle types.Throttle
}
type args struct {
steering float32
steering types.Steering
}
tests := []struct {
name string
@ -87,3 +91,134 @@ func TestSteeringProcessor_Process(t *testing.T) {
})
}
}
func TestSpeedZoneProcessor_Process(t *testing.T) {
type fields struct {
slowThrottle types.Throttle
normalThrottle types.Throttle
fastThrottle types.Throttle
speedZone events.SpeedZone
}
type args struct {
steering types.Steering
}
tests := []struct {
name string
fields fields
args args
want types.Throttle
}{
{
name: "steering straight, undefined zone",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: 0.},
want: 0.2,
},
{
name: "steering straight, slow zone",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: 0.},
want: 0.2,
},
{
name: "moderate left, slow speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: -0.5},
want: 0.2,
},
{
name: "moderate right, slow speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: 0.5},
want: 0.2,
},
{
name: "full left, slow speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: -0.95},
want: 0.2,
},
{
name: "full right, slow speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_SLOW},
args: args{steering: 0.95},
want: 0.2,
},
{
name: "steering straight, normal zone",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
args: args{steering: 0.},
want: 0.5,
},
{
name: "moderate left, normal speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
args: args{steering: -0.5},
want: 0.5,
},
{
name: "moderate right, normal speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
args: args{steering: 0.5},
want: 0.5,
},
{
name: "full left, normal speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
args: args{steering: -0.95},
want: 0.2,
},
{
name: "full right, normal speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_NORMAL},
args: args{steering: 0.95},
want: 0.2,
},
{
name: "steering straight, fast zone",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
args: args{steering: 0.},
want: 0.8,
},
{
name: "moderate left, fast speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
args: args{steering: -0.5},
want: 0.5,
},
{
name: "moderate right, fast speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
args: args{steering: 0.5},
want: 0.5,
},
{
name: "full left, fast speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
args: args{steering: -0.95},
want: 0.2,
},
{
name: "full right, fast speed",
fields: fields{slowThrottle: 0.2, normalThrottle: 0.5, fastThrottle: 0.8, speedZone: events.SpeedZone_FAST},
args: args{steering: 0.95},
want: 0.2,
},
}
for _, tt := range tests {
t.Run(tt.name, func(t *testing.T) {
sp := &SpeedZoneProcessor{
slowThrottle: tt.fields.slowThrottle,
normalThrottle: tt.fields.normalThrottle,
fastThrottle: tt.fields.fastThrottle,
moderateSteering: 0.4,
fullSteering: 0.8,
}
sp.SetSpeedZone(tt.fields.speedZone)
if got := sp.Process(tt.args.steering); got != tt.want {
t.Errorf("Process() = %v, want %v", got, tt.want)
}
})
}
}

2
pkg/types/types.go
View File

@ -1,3 +1,5 @@
package types
type Throttle float32
type Steering float32