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

Compare commits

base: robocars:838c9b4ef34ff0e8aadae7b0c3d173aee532f24c
Branches Tags
robocars:master
robocars:feat/brake
robocars:feat/use_steering_to_adjust_throttle
robocars:v0.9.0
robocars:v0.8.0
robocars:v0.7.0
robocars:v0.6.0
robocars:v0.5.0
robocars:v0.4.2
robocars:v0.4.1
robocars:v0.4.0
robocars:v0.3.0
robocars:v0.2.0
robocars:v0.1.0
..
compare: robocars:34c4433836ff89ef717331ae994682d77c9a7e1c
Branches Tags
robocars:master
robocars:feat/brake
robocars:feat/use_steering_to_adjust_throttle
robocars:v0.9.0
robocars:v0.8.0
robocars:v0.7.0
robocars:v0.6.0
robocars:v0.5.0
robocars:v0.4.2
robocars:v0.4.1
robocars:v0.4.0
robocars:v0.3.0
robocars:v0.2.0
robocars:v0.1.0

No commits in common. "838c9b4ef34ff0e8aadae7b0c3d173aee532f24c" and "34c4433836ff89ef717331ae994682d77c9a7e1c" have entirely different histories.
838c9b4ef3 ... 34c4433836

11 changed files with 48 additions and 479 deletions
Show Stats Expand all files Collapse all files

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

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

54
pkg/brake/config.go
View File

@ -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]
}

132
pkg/brake/config_test.go
View File

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

55
pkg/brake/controller.go
View File

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

92
pkg/brake/controller_test.go
View File

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

4
pkg/brake/test_data/config.json
View File

@ -1,4 +0,0 @@
{
"delta_steps": [ 0.05, 0.3, 0.5 ],
"data": [ -0.1, -0.5, -1.0 ]
}

52
pkg/throttle/controller.go
View File

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

49
pkg/throttle/controller_test.go
View File

@ -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
}
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},
},
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},
},
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},
},
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},
},
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},
},
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},
},
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

11
pkg/throttle/processor.go
View File

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

11
pkg/throttle/processor_test.go
View File

@ -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",

3
pkg/types/types.go
View File

@ -1,3 +0,0 @@
package types
type Throttle float32