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
Download Patch File Download Diff File Expand all files Collapse all files

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)
}
})
}
}