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

feat(brake): implement brake feature

Browse Source
This commit is contained in:
Cyrille Nofficial
2022-09-05 15:30:26 +02:00
parent dd0102ff44
commit 838c9b4ef3
11 changed files with 425 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
pkg/throttle/controller.go
View File

@ -3,6 +3,8 @@ 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"
@ -11,8 +13,8 @@ import (
)
func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic string,
minValue, maxValue float32, publishPilotFrequency int) *Controller {
return &Controller{
minValue, maxValue types.Throttle, publishPilotFrequency int, opts ...Option) *Controller {
c := &Controller{
client: client,
throttleTopic: throttleTopic,
driveModeTopic: driveModeTopic,
@ -23,14 +25,26 @@ func New(client mqtt.Client, throttleTopic, driveModeTopic, rcThrottleTopic, ste
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 float32
maxThrottle types.Throttle
steeringProcessor *SteeringProcessor
muDriveMode sync.RWMutex
@ -39,8 +53,7 @@ type Controller struct {
muSteering sync.RWMutex
steering float32
muThrottleFeedback sync.RWMutex
throttleFeedback float32
brakeCtrl brake.Controller
cancel chan interface{}
publishPilotFrequency int
@ -73,8 +86,10 @@ func (c *Controller) onPublishPilotValue() {
return
}
throttleFromSteering := c.steeringProcessor.Process(c.readSteering())
throttleMsg := events.ThrottleMessage{
Throttle: c.steeringProcessor.Process(c.readSteering()),
Throttle: float32(c.brakeCtrl.AdjustThrottle(throttleFromSteering)),
Confidence: 1.0,
}
payload, err := proto.Marshal(&throttleMsg)
@ -102,20 +117,17 @@ 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)
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", &msg, err)
return
}
c.muThrottleFeedback.Lock()
defer c.muThrottleFeedback.Unlock()
c.throttleFeedback = msg.GetThrottle()
c.brakeCtrl.SetRealThrottle(types.Throttle(msg.GetThrottle()))
}
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
}
@ -137,9 +149,9 @@ func (c *Controller) onRCThrottle(_ mqtt.Client, message mqtt.Message) {
return
}
zap.S().Debugf("publish new throttle value from rc: %v", throttleMsg.GetThrottle())
if throttleMsg.GetThrottle() > c.maxThrottle {
if types.Throttle(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 = c.maxThrottle
throttleMsg.Throttle = float32(c.maxThrottle)
payloadPatched, err := proto.Marshal(&throttleMsg)
if err != nil {
zap.S().Errorf("unable to marshall throttle msg: %v", err)
@ -165,13 +177,6 @@ func (c *Controller) onSteering(_ mqtt.Client, message mqtt.Message) {
c.steering = steeringMsg.GetSteering()
}
func (c *Controller) ThrottleFeedback() float32 {
c.muThrottleFeedback.RLock()
defer c.muThrottleFeedback.RUnlock()
tf := c.throttleFeedback
return tf
}
var registerCallbacks = func(p *Controller) error {
err := service.RegisterCallback(p.client, p.driveModeTopic, p.onDriveMode)
if err != nil {

35
pkg/throttle/controller_test.go
View File

@ -3,6 +3,8 @@ 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,13 +37,13 @@ func TestDefaultThrottle(t *testing.T) {
steeringTopic := "topic/rcThrottle"
throttleFeedbackTopic := "topic/feedback/throttle"
minValue := float32(0.56)
minValue := types.Throttle(0.56)
p := New(nil, throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic, minValue, 1., 200)
cases := []struct {
cases := []*struct {
name string
maxThrottle float32
maxThrottle types.Throttle
driveMode events.DriveModeMessage
rcThrottle events.ThrottleMessage
expectedThrottle events.ThrottleMessage
@ -118,8 +120,9 @@ func TestController_Start(t *testing.T) {
type fields struct {
driveMode events.DriveMode
min, max float32
min, max types.Throttle
publishPilotFrequency int
brakeCtl brake.Controller
}
type msgEvents struct {
driveMode *events.DriveModeMessage
@ -142,6 +145,7 @@ 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},
@ -158,6 +162,7 @@ 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},
@ -174,6 +179,7 @@ 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},
@ -190,6 +196,7 @@ 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},
@ -206,6 +213,7 @@ 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},
@ -222,6 +230,7 @@ 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},
@ -231,6 +240,23 @@ func TestController_Start(t *testing.T) {
},
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 {
@ -239,6 +265,7 @@ func TestController_Start(t *testing.T) {
throttleTopic, driveModeTopic, rcThrottleTopic, steeringTopic, throttleFeedbackTopic,
tt.fields.min, tt.fields.max,
tt.fields.publishPilotFrequency,
WithBrakeController(tt.fields.brakeCtl),
)
go c.Start()

11
pkg/throttle/processor.go
View File

@ -1,13 +1,16 @@
package throttle
import "math"
import (
"github.com/cyrilix/robocar-throttle/pkg/types"
"math"
)
type SteeringProcessor struct {
minThrottle, maxThrottle float32
minThrottle, maxThrottle types.Throttle
}
// Process compute throttle from steering value
func (sp *SteeringProcessor) Process(steering float32) float32 {
func (sp *SteeringProcessor) Process(steering float32) types.Throttle {
absSteering := math.Abs(float64(steering))
return sp.minThrottle + float32(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
return sp.minThrottle + types.Throttle(float64(sp.maxThrottle-sp.minThrottle)*(1-absSteering))
}

11
pkg/throttle/processor_test.go
View File

@ -1,11 +1,14 @@
package throttle
import "testing"
import (
"github.com/cyrilix/robocar-throttle/pkg/types"
"testing"
)
func TestSteeringProcessor_Process(t *testing.T) {
type fields struct {
minThrottle float32
maxThrottle float32
minThrottle types.Throttle
maxThrottle types.Throttle
}
type args struct {
steering float32
@ -14,7 +17,7 @@ func TestSteeringProcessor_Process(t *testing.T) {
name string
fields fields
args args
want float32
want types.Throttle
}{
{
name: "steering straight",