Compare commits
No commits in common. "838c9b4ef34ff0e8aadae7b0c3d173aee532f24c" and "34c4433836ff89ef717331ae994682d77c9a7e1c" have entirely different histories.
838c9b4ef3
...
34c4433836
@ -3,9 +3,7 @@ package main
|
||||
import (
|
||||
"flag"
|
||||
"github.com/cyrilix/robocar-base/cli"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/throttle"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"go.uber.org/zap"
|
||||
"log"
|
||||
"os"
|
||||
@ -18,11 +16,9 @@ const (
|
||||
|
||||
func main() {
|
||||
var mqttBroker, username, password, clientId string
|
||||
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
||||
var throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic string
|
||||
var minThrottle, maxThrottle float64
|
||||
var publishPilotFrequency int
|
||||
var brakeConfig string
|
||||
var enableBrake bool
|
||||
|
||||
err := cli.SetFloat64DefaultValueFromEnv(&minThrottle, "THROTTLE_MIN", DefaultThrottleMin)
|
||||
if err != nil {
|
||||
@ -42,14 +38,11 @@ func main() {
|
||||
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.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.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")
|
||||
|
||||
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`")
|
||||
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
|
||||
|
||||
flag.Parse()
|
||||
@ -77,14 +70,7 @@ func main() {
|
||||
}
|
||||
defer client.Disconnect(50)
|
||||
|
||||
var brakeCtrl brake.Controller
|
||||
if enableBrake {
|
||||
brakeCtrl = brake.NewCustomControllerWithJsonConfig(brakeConfig)
|
||||
} else {
|
||||
brakeCtrl = &brake.DisabledController{}
|
||||
}
|
||||
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
||||
types.Throttle(minThrottle), types.Throttle(maxThrottle), 2, throttle.WithBrakeController(brakeCtrl))
|
||||
p := throttle.New(client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, float32(minThrottle), float32(maxThrottle), 2)
|
||||
defer p.Stop()
|
||||
|
||||
cli.HandleExit(p)
|
||||
|
||||
@ -1,54 +0,0 @@
|
||||
package brake
|
||||
|
||||
import (
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"os"
|
||||
)
|
||||
|
||||
var (
|
||||
defaultBrakeConfig = Config{
|
||||
DeltaSteps: []float32{0.05, 0.3, 0.5},
|
||||
Data: []types.Throttle{-0.1, -0.5, -1.},
|
||||
}
|
||||
)
|
||||
|
||||
func NewConfig() *Config {
|
||||
return &defaultBrakeConfig
|
||||
}
|
||||
|
||||
func NewConfigFromJson(fileName string) (*Config, error) {
|
||||
content, err := os.ReadFile(fileName)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err)
|
||||
}
|
||||
var ft Config
|
||||
err = json.Unmarshal(content, &ft)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err)
|
||||
}
|
||||
return &ft, nil
|
||||
}
|
||||
|
||||
type Config struct {
|
||||
DeltaSteps []float32 `json:"delta_steps"`
|
||||
Data []types.Throttle `json:"data"`
|
||||
}
|
||||
|
||||
func (tc *Config) ValueOf(currentThrottle, targetThrottle types.Throttle) types.Throttle {
|
||||
delta := float32(currentThrottle - targetThrottle)
|
||||
|
||||
if delta < tc.DeltaSteps[0] {
|
||||
return targetThrottle
|
||||
}
|
||||
if delta >= tc.DeltaSteps[len(tc.DeltaSteps)-1] {
|
||||
return tc.Data[len(tc.Data)-1]
|
||||
}
|
||||
for idx, step := range tc.DeltaSteps {
|
||||
if delta < step {
|
||||
return tc.Data[idx-1]
|
||||
}
|
||||
}
|
||||
return tc.Data[len(tc.Data)-1]
|
||||
}
|
||||
@ -1,132 +0,0 @@
|
||||
package brake
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"reflect"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestNewConfigFromJson(t *testing.T) {
|
||||
type args struct {
|
||||
fileName string
|
||||
}
|
||||
tests := []struct {
|
||||
name string
|
||||
args args
|
||||
want *Config
|
||||
wantErr bool
|
||||
}{
|
||||
{
|
||||
name: "default config",
|
||||
args: args{
|
||||
fileName: "test_data/config.json",
|
||||
},
|
||||
want: &defaultBrakeConfig,
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
got, err := NewConfigFromJson(tt.args.fileName)
|
||||
if (err != nil) != tt.wantErr {
|
||||
t.Errorf("NewConfigFromJson() error = %v, wantErr %v", err, tt.wantErr)
|
||||
return
|
||||
}
|
||||
if !reflect.DeepEqual(*got, *tt.want) {
|
||||
t.Errorf("NewConfigFromJson() got = %v, want %v", got, tt.want)
|
||||
}
|
||||
if !reflect.DeepEqual(got.DeltaSteps, tt.want.DeltaSteps) {
|
||||
t.Errorf("NewConfigFromJson(), bad DeltaSteps: got = %v, want %v", got.DeltaSteps, tt.want.DeltaSteps)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestConfig_ValueOf(t *testing.T) {
|
||||
type fields struct {
|
||||
DeltaSteps []float32
|
||||
MinValue int
|
||||
Data []types.Throttle
|
||||
}
|
||||
type args struct {
|
||||
currentThrottle, targetThrottle types.Throttle
|
||||
}
|
||||
tests := []struct {
|
||||
name string
|
||||
fields fields
|
||||
args args
|
||||
want types.Throttle
|
||||
}{
|
||||
{
|
||||
name: "delta > 0",
|
||||
fields: fields{
|
||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||
Data: defaultBrakeConfig.Data,
|
||||
},
|
||||
args: args{
|
||||
currentThrottle: 0.5,
|
||||
targetThrottle: 0.8,
|
||||
},
|
||||
want: 0.8,
|
||||
},
|
||||
{
|
||||
name: "no delta",
|
||||
fields: fields{
|
||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||
Data: defaultBrakeConfig.Data,
|
||||
},
|
||||
args: args{
|
||||
currentThrottle: 0.5,
|
||||
targetThrottle: 0.5,
|
||||
},
|
||||
want: 0.5,
|
||||
},
|
||||
{
|
||||
name: "delta very low (< 1st step)",
|
||||
fields: fields{
|
||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||
Data: defaultBrakeConfig.Data,
|
||||
},
|
||||
args: args{
|
||||
currentThrottle: 0.5,
|
||||
targetThrottle: 0.495,
|
||||
},
|
||||
want: 0.495,
|
||||
},
|
||||
{
|
||||
name: "low delta ( 1st step < delta < 2nd step )",
|
||||
fields: fields{
|
||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||
Data: defaultBrakeConfig.Data,
|
||||
},
|
||||
args: args{
|
||||
currentThrottle: 0.5,
|
||||
targetThrottle: 0.38,
|
||||
},
|
||||
want: -0.1,
|
||||
},
|
||||
{
|
||||
name: "high delta",
|
||||
fields: fields{
|
||||
DeltaSteps: defaultBrakeConfig.DeltaSteps,
|
||||
Data: defaultBrakeConfig.Data,
|
||||
},
|
||||
args: args{
|
||||
currentThrottle: 0.8,
|
||||
targetThrottle: 0.3,
|
||||
},
|
||||
want: -1.,
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
f := &Config{
|
||||
DeltaSteps: tt.fields.DeltaSteps,
|
||||
Data: tt.fields.Data,
|
||||
}
|
||||
got := f.ValueOf(tt.args.currentThrottle, tt.args.targetThrottle)
|
||||
if !reflect.DeepEqual(got, tt.want) {
|
||||
t.Errorf("ValueOf() = %v, want %v", got, tt.want)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@ -1,55 +0,0 @@
|
||||
package brake
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"go.uber.org/zap"
|
||||
"sync"
|
||||
)
|
||||
|
||||
type Controller interface {
|
||||
SetRealThrottle(t types.Throttle)
|
||||
AdjustThrottle(targetThrottle types.Throttle) types.Throttle
|
||||
}
|
||||
|
||||
func NewCustomController() *CustomController {
|
||||
return &CustomController{cfg: NewConfig()}
|
||||
}
|
||||
|
||||
func NewCustomControllerWithJsonConfig(filename string) *CustomController {
|
||||
config, err := NewConfigFromJson(filename)
|
||||
if err != nil {
|
||||
zap.S().Panicf("unable to init brake controller with json config '%s': %v", filename, err)
|
||||
}
|
||||
return &CustomController{cfg: config}
|
||||
}
|
||||
|
||||
type CustomController struct {
|
||||
muRealThrottle sync.RWMutex
|
||||
realThrottle types.Throttle
|
||||
cfg *Config
|
||||
}
|
||||
|
||||
func (b *CustomController) SetRealThrottle(t types.Throttle) {
|
||||
b.muRealThrottle.Lock()
|
||||
defer b.muRealThrottle.Unlock()
|
||||
b.realThrottle = t
|
||||
}
|
||||
|
||||
func (b *CustomController) GetRealThrottle() types.Throttle {
|
||||
b.muRealThrottle.RLock()
|
||||
defer b.muRealThrottle.RUnlock()
|
||||
res := b.realThrottle
|
||||
return res
|
||||
}
|
||||
|
||||
func (b *CustomController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
||||
return b.cfg.ValueOf(b.GetRealThrottle(), targetThrottle)
|
||||
}
|
||||
|
||||
type DisabledController struct{}
|
||||
|
||||
func (d *DisabledController) SetRealThrottle(_ types.Throttle) {}
|
||||
|
||||
func (d *DisabledController) AdjustThrottle(targetThrottle types.Throttle) types.Throttle {
|
||||
return targetThrottle
|
||||
}
|
||||
@ -1,92 +0,0 @@
|
||||
package brake
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestController_AdjustThrottle(t *testing.T) {
|
||||
type fields struct {
|
||||
realThrottle types.Throttle
|
||||
}
|
||||
type args struct {
|
||||
targetThrottle types.Throttle
|
||||
}
|
||||
tests := []struct {
|
||||
name string
|
||||
fields fields
|
||||
args args
|
||||
want types.Throttle
|
||||
}{
|
||||
{
|
||||
name: "target same as current throttle",
|
||||
fields: fields{realThrottle: 0.2},
|
||||
args: args{targetThrottle: 0.2},
|
||||
want: 0.2,
|
||||
},
|
||||
{
|
||||
name: "target > as current throttle",
|
||||
fields: fields{realThrottle: 0.2},
|
||||
args: args{targetThrottle: 0.3},
|
||||
want: 0.3,
|
||||
},
|
||||
{
|
||||
name: "target >> as current throttle",
|
||||
fields: fields{realThrottle: 0.2},
|
||||
args: args{targetThrottle: 0.5},
|
||||
want: 0.5,
|
||||
},
|
||||
{
|
||||
name: "target < as current throttle",
|
||||
fields: fields{realThrottle: 0.8},
|
||||
args: args{targetThrottle: 0.7},
|
||||
want: -0.1,
|
||||
},
|
||||
{
|
||||
name: "target << as current throttle",
|
||||
fields: fields{realThrottle: 0.8},
|
||||
args: args{targetThrottle: 0.5},
|
||||
want: -0.5,
|
||||
},
|
||||
{
|
||||
name: "target <<< as current throttle",
|
||||
fields: fields{realThrottle: 0.8},
|
||||
args: args{targetThrottle: 0.2},
|
||||
want: -1.,
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
b := &CustomController{cfg: NewConfig()}
|
||||
b.SetRealThrottle(tt.fields.realThrottle)
|
||||
if got := b.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
||||
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestDisabledController_AdjustThrottle(t *testing.T) {
|
||||
type args struct {
|
||||
targetThrottle types.Throttle
|
||||
}
|
||||
tests := []struct {
|
||||
name string
|
||||
args args
|
||||
want types.Throttle
|
||||
}{
|
||||
{
|
||||
name: "doesn't modify value",
|
||||
args: args{targetThrottle: 0.5},
|
||||
want: 0.5,
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
d := &DisabledController{}
|
||||
if got := d.AdjustThrottle(tt.args.targetThrottle); got != tt.want {
|
||||
t.Errorf("AdjustThrottle() = %v, want %v", got, tt.want)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
@ -1,4 +0,0 @@
|
||||
{
|
||||
"delta_steps": [ 0.05, 0.3, 0.5 ],
|
||||
"data": [ -0.1, -0.5, -1.0 ]
|
||||
}
|
||||
@ -3,8 +3,6 @@ package throttle
|
||||
import (
|
||||
"github.com/cyrilix/robocar-base/service"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||
"go.uber.org/zap"
|
||||
"google.golang.org/protobuf/proto"
|
||||
@ -12,39 +10,25 @@ import (
|
||||
"time"
|
||||
)
|
||||
|
||||
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
|
||||
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
|
||||
c := &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,
|
||||
throttleFeedbackTopic: throttleFeedbackTopic,
|
||||
maxThrottle: maxValue,
|
||||
driveMode: events.DriveMode_USER,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
steeringProcessor: &SteeringProcessor{minThrottle: minValue, maxThrottle: maxValue},
|
||||
brakeCtrl: &brake.DisabledController{},
|
||||
}
|
||||
for _, o := range opts {
|
||||
o(c)
|
||||
}
|
||||
return c
|
||||
}
|
||||
|
||||
type Option func(c *Controller)
|
||||
|
||||
func WithBrakeController(bc brake.Controller) Option {
|
||||
return func(c *Controller) {
|
||||
c.brakeCtrl = bc
|
||||
}
|
||||
}
|
||||
|
||||
type Controller struct {
|
||||
client mqtt.Client
|
||||
throttleTopic string
|
||||
maxThrottle types.Throttle
|
||||
maxThrottle float32
|
||||
steeringProcessor *SteeringProcessor
|
||||
|
||||
muDriveMode sync.RWMutex
|
||||
@ -53,11 +37,9 @@ type Controller struct {
|
||||
muSteering sync.RWMutex
|
||||
steering float32
|
||||
|
||||
brakeCtrl brake.Controller
|
||||
|
||||
cancel chan interface{}
|
||||
publishPilotFrequency int
|
||||
driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string
|
||||
cancel chan interface{}
|
||||
publishPilotFrequency int
|
||||
driveModeTopic, rcThrottleTopic, steeringTopic string
|
||||
}
|
||||
|
||||
func (c *Controller) Start() error {
|
||||
@ -86,10 +68,8 @@ func (c *Controller) onPublishPilotValue() {
|
||||
return
|
||||
}
|
||||
|
||||
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
|
||||
|
||||
throttleMsg := events.ThrottleMessage{
|
||||
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
|
||||
Throttle: c.steeringProcessor.Process(c.readSteering()),
|
||||
Confidence: 1.0,
|
||||
}
|
||||
payload, err := proto.Marshal(&throttleMsg)
|
||||
@ -110,24 +90,14 @@ 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)
|
||||
}
|
||||
|
||||
func (c *Controller) onThrottleFeedback(_ mqtt.Client, message mqtt.Message) {
|
||||
var msg events.ThrottleMessage
|
||||
err := proto.Unmarshal(message.Payload(), &msg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
|
||||
return
|
||||
}
|
||||
c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
|
||||
service.StopService("throttle", c.client, c.driveModeTopic, c.rcThrottleTopic, c.steeringTopic)
|
||||
}
|
||||
|
||||
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
|
||||
var msg events.DriveModeMessage
|
||||
err := proto.Unmarshal(message.Payload(), &msg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
|
||||
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
|
||||
return
|
||||
}
|
||||
|
||||
@ -149,9 +119,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
|
||||
return
|
||||
}
|
||||
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
|
||||
if types.Throttle(throttleMsg.GetThrottle()) > c.maxThrottle {
|
||||
if throttleMsg.GetThrottle() > c.maxThrottle {
|
||||
zap.S().Debugf("throttle upper that max value allowed, patch value from %v to %v", throttleMsg.GetThrottle(), c.maxThrottle)
|
||||
throttleMsg.Throttle = float32(c.maxThrottle)
|
||||
throttleMsg.Throttle = c.maxThrottle
|
||||
payloadPatched, err := proto.Marshal(&throttleMsg)
|
||||
if err != nil {
|
||||
zap.S().Errorf("unable to marshall throttle msg: %v", err)
|
||||
@ -192,10 +162,6 @@ var registerCallbacks = func(p *Controller) error {
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
err = service.RegisterCallback(p.client, p.throttleFeedbackTopic, p.onThrottleFeedback)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
|
||||
@ -3,8 +3,6 @@ package throttle
|
||||
import (
|
||||
"github.com/cyrilix/robocar-base/testtools"
|
||||
"github.com/cyrilix/robocar-protobuf/go/events"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/brake"
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
mqtt "github.com/eclipse/paho.mqtt.golang"
|
||||
"google.golang.org/protobuf/proto"
|
||||
"sync"
|
||||
@ -35,15 +33,14 @@ func TestDefaultThrottle(t *testing.T) {
|
||||
driveModeTopic := "topic/driveMode"
|
||||
rcThrottleTopic := "topic/rcThrottle"
|
||||
steeringTopic := "topic/rcThrottle"
|
||||
throttleFeedbackTopic := "topic/feedback/throttle"
|
||||
|
||||
minValue := types.Throttle(0.56)
|
||||
minValue := float32(0.56)
|
||||
|
||||
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
|
||||
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, minValue, 1., 200)
|
||||
|
||||
cases := []*struct {
|
||||
cases := []struct {
|
||||
name string
|
||||
maxThrottle types.Throttle
|
||||
maxThrottle float32
|
||||
driveMode events.DriveModeMessage
|
||||
rcThrottle events.ThrottleMessage
|
||||
expectedThrottle events.ThrottleMessage
|
||||
@ -116,19 +113,16 @@ func TestController_Start(t *testing.T) {
|
||||
steeringTopic := "topic/steering"
|
||||
driveModeTopic := "topic/driveMode"
|
||||
rcThrottleTopic := "topic/rcThrottle"
|
||||
throttleFeedbackTopic := "topic/feedback/throttle"
|
||||
|
||||
type fields struct {
|
||||
driveMode events.DriveMode
|
||||
min, max types.Throttle
|
||||
min, max float32
|
||||
publishPilotFrequency int
|
||||
brakeCtl brake.Controller
|
||||
}
|
||||
type msgEvents struct {
|
||||
driveMode *events.DriveModeMessage
|
||||
steering *events.SteeringMessage
|
||||
rcThrottle *events.ThrottleMessage
|
||||
throttleFeedback *events.ThrottleMessage
|
||||
driveMode *events.DriveModeMessage
|
||||
steering *events.SteeringMessage
|
||||
rcThrottle *events.ThrottleMessage
|
||||
}
|
||||
|
||||
tests := []struct {
|
||||
@ -145,13 +139,11 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
|
||||
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},
|
||||
},
|
||||
@ -162,13 +154,11 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.8, Confidence: 1.0},
|
||||
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},
|
||||
},
|
||||
@ -179,13 +169,11 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
|
||||
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},
|
||||
},
|
||||
@ -196,13 +184,11 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
|
||||
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},
|
||||
},
|
||||
@ -213,13 +199,11 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
|
||||
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},
|
||||
},
|
||||
@ -230,42 +214,22 @@ func TestController_Start(t *testing.T) {
|
||||
max: 0.8,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: &brake.DisabledController{},
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 0.4, Confidence: 1.0},
|
||||
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, should brake on brutal change",
|
||||
fields: fields{
|
||||
driveMode: events.DriveMode_PILOT,
|
||||
max: 1.0,
|
||||
min: 0.3,
|
||||
publishPilotFrequency: publishPilotFrequency,
|
||||
brakeCtl: brake.NewCustomController(),
|
||||
},
|
||||
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},
|
||||
throttleFeedback: &events.ThrottleMessage{Throttle: 1.0, Confidence: 1.0},
|
||||
},
|
||||
want: &events.ThrottleMessage{Throttle: -1.0, Confidence: 1.0},
|
||||
},
|
||||
}
|
||||
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
c := New(nil,
|
||||
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
|
||||
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic,
|
||||
tt.fields.min, tt.fields.max,
|
||||
tt.fields.publishPilotFrequency,
|
||||
WithBrakeController(tt.fields.brakeCtl),
|
||||
)
|
||||
|
||||
go c.Start()
|
||||
@ -277,7 +241,6 @@ func TestController_Start(t *testing.T) {
|
||||
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))
|
||||
c.onThrottleFeedback(nil, testtools.NewFakeMessageFromProtobuf(throttleFeedbackTopic, tt.msgEvents.throttleFeedback))
|
||||
waitPublish.Wait()
|
||||
|
||||
var msg events.ThrottleMessage
|
||||
|
||||
@ -1,16 +1,13 @@
|
||||
package throttle
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"math"
|
||||
)
|
||||
import "math"
|
||||
|
||||
type SteeringProcessor struct {
|
||||
minThrottle, maxThrottle types.Throttle
|
||||
minThrottle, maxThrottle float32
|
||||
}
|
||||
|
||||
// Process compute throttle from steering value
|
||||
func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
|
||||
func (sp *SteeringProcessor) Process(steering float32) float32 {
|
||||
absSteering := math.Abs(float64(steering))
|
||||
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
||||
return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
|
||||
}
|
||||
|
||||
@ -1,14 +1,11 @@
|
||||
package throttle
|
||||
|
||||
import (
|
||||
"github.com/cyrilix/robocar-throttle/pkg/types"
|
||||
"testing"
|
||||
)
|
||||
import "testing"
|
||||
|
||||
func TestSteeringProcessor_Process(t *testing.T) {
|
||||
type fields struct {
|
||||
minThrottle types.Throttle
|
||||
maxThrottle types.Throttle
|
||||
minThrottle float32
|
||||
maxThrottle float32
|
||||
}
|
||||
type args struct {
|
||||
steering float32
|
||||
@ -17,7 +14,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
|
||||
name string
|
||||
fields fields
|
||||
args args
|
||||
want types.Throttle
|
||||
want float32
|
||||
}{
|
||||
{
|
||||
name: "steering straight",
|
||||
|
||||
@ -1,3 +0,0 @@
|
||||
package types
|
||||
|
||||
type Throttle float32
|
||||
Loading…
Reference in New Issue
Block a user