feat: record autopilot steering and driveMode

This commit is contained in:
Cyrille Nofficial 2023-01-25 19:21:20 +01:00
parent a056c6f1b8
commit eac2806072
7 changed files with 226 additions and 75 deletions

View File

@ -15,7 +15,7 @@ const (
func main() { func main() {
var mqttBroker, username, password, clientId string var mqttBroker, username, password, clientId string
var cameraTopic, steeringTopic, recordTopic, switchRecordTopic string var cameraTopic, rcSteeringTopic, recordTopic, tfSteeringTopic, driveModeTopic, switchRecordTopic string
var debug bool var debug bool
mqttQos := cli.InitIntFlag("MQTT_QOS", 0) mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
@ -24,7 +24,9 @@ func main() {
cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain) cli.InitMqttFlags(DefaultClientId, &mqttBroker, &username, &password, &clientId, &mqttQos, &mqttRetain)
flag.StringVar(&cameraTopic, "mqtt-topic-frame", os.Getenv("MQTT_TOPIC_CAMERA"), "Mqtt topic that contains camera frame, use MQTT_TOPIC_CAMERA if args not set") flag.StringVar(&cameraTopic, "mqtt-topic-frame", os.Getenv("MQTT_TOPIC_CAMERA"), "Mqtt topic that contains camera frame, use MQTT_TOPIC_CAMERA if args not set")
flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering raw values, use MQTT_TOPIC_STEERING if args not set") flag.StringVar(&rcSteeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic that contains steering raw values, use MQTT_TOPIC_STEERING if args not set")
flag.StringVar(&tfSteeringTopic, "mqtt-topic-autopilot-steering", os.Getenv("MQTT_TOPIC_AUTOPILOT_STEERING"), "Mqtt topic that contains steering computed by autopilot, use MQTT_TOPIC_AUTOPILOT_STEERING if args not set")
flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains drive mode instruction, use MQTT_TOPIC_DRIVE_MODE if args not set")
flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic that contain switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set") flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic that contain switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set")
flag.StringVar(&recordTopic, "mqtt-topic-records", os.Getenv("MQTT_TOPIC_RECORDS"), "Mqtt topic to publish record messages, use MQTT_TOPIC_RECORDS if args not set") flag.StringVar(&recordTopic, "mqtt-topic-records", os.Getenv("MQTT_TOPIC_RECORDS"), "Mqtt topic to publish record messages, use MQTT_TOPIC_RECORDS if args not set")
flag.BoolVar(&debug, "debug", false, "Display raw value to debug") flag.BoolVar(&debug, "debug", false, "Display raw value to debug")
@ -58,7 +60,14 @@ func main() {
} }
defer client.Disconnect(50) defer client.Disconnect(50)
r := part.NewRecorder(client, recordTopic, cameraTopic, steeringTopic, switchRecordTopic) r := part.NewRecorder(client,
recordTopic,
cameraTopic,
rcSteeringTopic,
tfSteeringTopic,
driveModeTopic,
switchRecordTopic,
)
defer r.Stop() defer r.Stop()
cli.HandleExit(r) cli.HandleExit(r)

2
go.mod
View File

@ -4,7 +4,7 @@ go 1.19
require ( require (
github.com/cyrilix/robocar-base v0.1.7 github.com/cyrilix/robocar-base v0.1.7
github.com/cyrilix/robocar-protobuf/go v1.0.5 github.com/cyrilix/robocar-protobuf/go v1.2.0
github.com/eclipse/paho.mqtt.golang v1.4.1 github.com/eclipse/paho.mqtt.golang v1.4.1
go.uber.org/zap v1.21.0 go.uber.org/zap v1.21.0
google.golang.org/protobuf v1.28.0 google.golang.org/protobuf v1.28.0

4
go.sum
View File

@ -2,8 +2,8 @@ github.com/benbjohnson/clock v1.1.0 h1:Q92kusRqC1XV2MjkWETPvjJVqKetz1OzxZB7mHJLj
github.com/benbjohnson/clock v1.1.0/go.mod h1:J11/hYXuz8f4ySSvYwY0FKfm+ezbsZBKZxNJlLklBHA= github.com/benbjohnson/clock v1.1.0/go.mod h1:J11/hYXuz8f4ySSvYwY0FKfm+ezbsZBKZxNJlLklBHA=
github.com/cyrilix/robocar-base v0.1.7 h1:EVzZ0KjigSFpke5f3A/PybEH3WFUEIrYSc3z/dhOZ48= github.com/cyrilix/robocar-base v0.1.7 h1:EVzZ0KjigSFpke5f3A/PybEH3WFUEIrYSc3z/dhOZ48=
github.com/cyrilix/robocar-base v0.1.7/go.mod h1:4E11HQSNy2NT8e7MW188y6ST9C0RzarKyn7sK/3V/Lk= github.com/cyrilix/robocar-base v0.1.7/go.mod h1:4E11HQSNy2NT8e7MW188y6ST9C0RzarKyn7sK/3V/Lk=
github.com/cyrilix/robocar-protobuf/go v1.0.5 h1:PX1At+pf6G7gJwT4LzJLQu3/LPFTTNNlZmZSYtnSELY= github.com/cyrilix/robocar-protobuf/go v1.2.0 h1:qxTm7X72rizaR8lO5ZpCMOoZv+ix24piiAv8w9Bh5tE=
github.com/cyrilix/robocar-protobuf/go v1.0.5/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ= github.com/cyrilix/robocar-protobuf/go v1.2.0/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c= github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c=
github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=

View File

@ -11,12 +11,15 @@ import (
"time" "time"
) )
func NewRecorder(client mqtt.Client, recordTopic, cameraTopic, steeringTopic, switchRecordTopic string) *Recorder { func NewRecorder(client mqtt.Client, recordTopic, cameraTopic, rcSteeringTopic, tfSteeringTopic, driveModeTopic,
switchRecordTopic string) *Recorder {
return &Recorder{ return &Recorder{
client: client, client: client,
recordTopic: recordTopic, recordTopic: recordTopic,
cameraTopic: cameraTopic, cameraTopic: cameraTopic,
steeringTopic: steeringTopic, rcSteeringTopic: rcSteeringTopic,
tfSteeringTopic: tfSteeringTopic,
driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic, switchRecordTopic: switchRecordTopic,
enabled: false, enabled: false,
idGenerator: NewDateBasedGenerator(), idGenerator: NewDateBasedGenerator(),
@ -28,10 +31,18 @@ func NewRecorder(client mqtt.Client, recordTopic, cameraTopic, steeringTopic, sw
type Recorder struct { type Recorder struct {
client mqtt.Client client mqtt.Client
recordTopic string recordTopic string
cameraTopic, steeringTopic, switchRecordTopic string cameraTopic, switchRecordTopic string
muSteeringMsg sync.Mutex driveModeTopic, rcSteeringTopic, tfSteeringTopic string
currentSteering *events.SteeringMessage
muRcSteeringMsg sync.Mutex
currentRcSteering *events.SteeringMessage
muTfSteeringMsg sync.Mutex
currentTfSteering *events.SteeringMessage
muDriveModeMsg sync.Mutex
currentDriveMode *events.DriveModeMessage
muEnabled sync.RWMutex muEnabled sync.RWMutex
enabled bool enabled bool
@ -57,7 +68,7 @@ func (r *Recorder) Start() error {
func (r *Recorder) Stop() { func (r *Recorder) Stop() {
close(r.cancel) close(r.cancel)
service.StopService("record", r.client, r.cameraTopic, r.steeringTopic) service.StopService("record", r.client, r.cameraTopic, r.rcSteeringTopic, r.tfSteeringTopic, r.driveModeTopic)
} }
func (r *Recorder) onSwitchRecord(_ mqtt.Client, message mqtt.Message) { func (r *Recorder) onSwitchRecord(_ mqtt.Client, message mqtt.Message) {
@ -78,7 +89,7 @@ func (r *Recorder) onSwitchRecord(_ mqtt.Client, message mqtt.Message) {
r.enabled = msg.GetEnabled() r.enabled = msg.GetEnabled()
} }
func (r *Recorder) onSteering(_ mqtt.Client, message mqtt.Message) { func (r *Recorder) onRcSteering(_ mqtt.Client, message mqtt.Message) {
var msg events.SteeringMessage var msg events.SteeringMessage
err := proto.Unmarshal(message.Payload(), &msg) err := proto.Unmarshal(message.Payload(), &msg)
if err != nil { if err != nil {
@ -86,9 +97,35 @@ func (r *Recorder) onSteering(_ mqtt.Client, message mqtt.Message) {
return return
} }
r.muSteeringMsg.Lock() r.muRcSteeringMsg.Lock()
defer r.muSteeringMsg.Unlock() defer r.muRcSteeringMsg.Unlock()
r.currentSteering = &msg r.currentRcSteering = &msg
}
func (r *Recorder) onTfSteering(_ mqtt.Client, message mqtt.Message) {
var msg events.SteeringMessage
err := proto.Unmarshal(message.Payload(), &msg)
if err != nil {
zap.S().Errorf("unable to unmarshal protobuf %T: %v", msg, err)
return
}
r.muTfSteeringMsg.Lock()
defer r.muTfSteeringMsg.Unlock()
r.currentTfSteering = &msg
}
func (r *Recorder) 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: %v", msg, err)
return
}
r.muDriveModeMsg.Lock()
defer r.muDriveModeMsg.Unlock()
r.currentDriveMode = &msg
} }
func (r *Recorder) onFrame(_ mqtt.Client, message mqtt.Message) { func (r *Recorder) onFrame(_ mqtt.Client, message mqtt.Message) {
@ -109,9 +146,21 @@ func (r *Recorder) onFrame(_ mqtt.Client, message mqtt.Message) {
return return
} }
autopilot := r.CurrentAutopilotSteering()
if autopilot == nil {
zap.S().Warnf("no current autopilot steeringMsg")
}
driveMode := r.CurrentDriveMode()
if driveMode == nil {
zap.S().Warnf("no current driveModeMsg")
}
record := events.RecordMessage{ record := events.RecordMessage{
Frame: &msg, Frame: &msg,
Steering: steering, Steering: steering,
AutopilotSteering: autopilot,
DriveMode: driveMode,
RecordSet: r.recordSet, RecordSet: r.recordSet,
} }
@ -128,12 +177,26 @@ var publish = func(client mqtt.Client, topic string, payload *[]byte) {
} }
func (r *Recorder) CurrentSteering() *events.SteeringMessage { func (r *Recorder) CurrentSteering() *events.SteeringMessage {
r.muSteeringMsg.Lock() r.muRcSteeringMsg.Lock()
defer r.muSteeringMsg.Unlock() defer r.muRcSteeringMsg.Unlock()
steering := r.currentSteering steering := r.currentRcSteering
return steering return steering
} }
func (r *Recorder) CurrentAutopilotSteering() *events.SteeringMessage {
r.muTfSteeringMsg.Lock()
defer r.muTfSteeringMsg.Unlock()
steering := r.currentTfSteering
return steering
}
func (r *Recorder) CurrentDriveMode() *events.DriveModeMessage {
r.muDriveModeMsg.Lock()
defer r.muDriveModeMsg.Unlock()
driveMode := r.currentDriveMode
return driveMode
}
func (r *Recorder) Enabled() bool { func (r *Recorder) Enabled() bool {
r.muEnabled.RLock() r.muEnabled.RLock()
defer r.muEnabled.RUnlock() defer r.muEnabled.RUnlock()
@ -146,9 +209,19 @@ var registerCallBacks = func(r *Recorder) {
zap.S().Panicf("unable to register callback to %v:%v", r.cameraTopic, err) zap.S().Panicf("unable to register callback to %v:%v", r.cameraTopic, err)
} }
err = service.RegisterCallback(r.client, r.steeringTopic, r.onSteering) err = service.RegisterCallback(r.client, r.rcSteeringTopic, r.onRcSteering)
if err != nil { if err != nil {
zap.S().Panicf("unable to register callback to %v:%v", r.steeringTopic, err) zap.S().Panicf("unable to register callback to %v:%v", r.rcSteeringTopic, err)
}
err = service.RegisterCallback(r.client, r.tfSteeringTopic, r.onTfSteering)
if err != nil {
zap.S().Panicf("unable to register callback to %v:%v", r.tfSteeringTopic, err)
}
err = service.RegisterCallback(r.client, r.driveModeTopic, r.onDriveMode)
if err != nil {
zap.S().Panicf("unable to register callback to %v:%v", r.driveModeTopic, err)
} }
err = service.RegisterCallback(r.client, r.switchRecordTopic, r.onSwitchRecord) err = service.RegisterCallback(r.client, r.switchRecordTopic, r.onSwitchRecord)

View File

@ -6,7 +6,7 @@ import (
mqtt "github.com/eclipse/paho.mqtt.golang" mqtt "github.com/eclipse/paho.mqtt.golang"
"go.uber.org/zap" "go.uber.org/zap"
"google.golang.org/protobuf/proto" "google.golang.org/protobuf/proto"
"io/ioutil" "os"
"regexp" "regexp"
"strings" "strings"
"sync" "sync"
@ -27,6 +27,8 @@ func TestRecorder_Record(t *testing.T) {
recordTopic := "topic/record" recordTopic := "topic/record"
cameraTopic := "topic/camera" cameraTopic := "topic/camera"
steeringTopic := "topic/steeringMsg" steeringTopic := "topic/steeringMsg"
autopilotSteeringTopic := "topic/autopilot/steering"
driveModeTopic := "topic/driveMode"
switchRecord := "topic/switch/record" switchRecord := "topic/switch/record"
var muEventsPublished sync.Mutex var muEventsPublished sync.Mutex
@ -46,7 +48,13 @@ func TestRecorder_Record(t *testing.T) {
eventsPublished = &msg eventsPublished = &msg
} }
recorder := NewRecorder(nil, recordTopic, cameraTopic, steeringTopic, switchRecord) recorder := NewRecorder(nil,
recordTopic,
cameraTopic,
steeringTopic,
autopilotSteeringTopic,
driveModeTopic,
switchRecord)
recorder.idGenerator = &DateBasedGenerator{ recorder.idGenerator = &DateBasedGenerator{
muCpt: sync.Mutex{}, muCpt: sync.Mutex{},
cpt: 0, cpt: 0,
@ -64,24 +72,52 @@ func TestRecorder_Record(t *testing.T) {
frame2 := loadImage(t, "testdata/img.jpg", "02") frame2 := loadImage(t, "testdata/img.jpg", "02")
steeringRight := events.SteeringMessage{Steering: 0.5, Confidence: 1.0} steeringRight := events.SteeringMessage{Steering: 0.5, Confidence: 1.0}
steeringLeft := events.SteeringMessage{Steering: -0.5, Confidence: 1.0} steeringLeft := events.SteeringMessage{Steering: -0.5, Confidence: 1.0}
autopilotLeft := events.SteeringMessage{Steering: -0.8, Confidence: 1.0}
driveModeManuel := events.DriveModeMessage{DriveMode: events.DriveMode_USER}
cases := []struct { cases := []struct {
recordMsg *events.SwitchRecordMessage recordMsg *events.SwitchRecordMessage
frameMsg *events.FrameMessage frameMsg *events.FrameMessage
steeringMsg *events.SteeringMessage steeringMsg *events.SteeringMessage
autopilotMsg *events.SteeringMessage
driveModeMsg *events.DriveModeMessage
expectedRecordMsg *events.RecordMessage expectedRecordMsg *events.RecordMessage
wait time.Duration wait time.Duration
}{ }{
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, {recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil,
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil, steeringMsg: &steeringRight, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, {recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil,
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1, steeringMsg: &steeringRight, expectedRecordMsg: &events.RecordMessage{RecordSet: "record-1", Frame: frame1, Steering: &steeringRight}, wait: 5 * time.Millisecond}, steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil, steeringMsg: &steeringLeft, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame2, steeringMsg: &steeringLeft, expectedRecordMsg: &events.RecordMessage{RecordSet: "record-1", Frame: frame2, Steering: &steeringLeft}, wait: 5 * time.Millisecond}, {recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1,
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond}, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1, steeringMsg: &steeringRight, expectedRecordMsg: &events.RecordMessage{RecordSet: "record-2", Frame: frame1, Steering: &steeringLeft}, wait: 5 * time.Millisecond}, {recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil,
steeringMsg: &steeringRight, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1,
steeringMsg: &steeringRight, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: &events.RecordMessage{RecordSet: "record-1", Frame: frame1, Steering: &steeringRight}, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil,
steeringMsg: &steeringLeft, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame2,
steeringMsg: &steeringLeft, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: &events.RecordMessage{RecordSet: "record-1", Frame: frame2, Steering: &steeringLeft}, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil,
steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil,
steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1,
steeringMsg: &steeringRight, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: &events.RecordMessage{RecordSet: "record-2", Frame: frame1, Steering: &steeringLeft}, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1,
steeringMsg: &steeringRight, autopilotMsg: &autopilotLeft, driveModeMsg: &driveModeManuel,
expectedRecordMsg: &events.RecordMessage{RecordSet: "record-2", Frame: frame1, Steering: &steeringRight, AutopilotSteering: &autopilotLeft, DriveMode: &driveModeManuel},
wait: 5 * time.Millisecond},
} }
for _, c := range cases { for _, c := range cases {
@ -89,6 +125,12 @@ func TestRecorder_Record(t *testing.T) {
eventsPublished = nil eventsPublished = nil
muEventsPublished.Unlock() muEventsPublished.Unlock()
if c.autopilotMsg != nil {
recorder.onTfSteering(nil, testtools.NewFakeMessageFromProtobuf(autopilotSteeringTopic, c.autopilotMsg))
}
if c.driveModeMsg != nil {
recorder.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, c.driveModeMsg))
}
if c.recordMsg != nil { if c.recordMsg != nil {
recorder.onSwitchRecord(nil, testtools.NewFakeMessageFromProtobuf(recordTopic, c.recordMsg)) recorder.onSwitchRecord(nil, testtools.NewFakeMessageFromProtobuf(recordTopic, c.recordMsg))
} }
@ -96,7 +138,7 @@ func TestRecorder_Record(t *testing.T) {
recorder.onFrame(nil, testtools.NewFakeMessageFromProtobuf(cameraTopic, c.frameMsg)) recorder.onFrame(nil, testtools.NewFakeMessageFromProtobuf(cameraTopic, c.frameMsg))
} }
if c.steeringMsg != nil { if c.steeringMsg != nil {
recorder.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, c.steeringMsg)) recorder.onRcSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, c.steeringMsg))
} }
time.Sleep(c.wait) time.Sleep(c.wait)
@ -112,7 +154,7 @@ func TestRecorder_Record(t *testing.T) {
} }
func loadImage(t *testing.T, imgPath string, id string) *events.FrameMessage { func loadImage(t *testing.T, imgPath string, id string) *events.FrameMessage {
jpegContent, err := ioutil.ReadFile(imgPath) jpegContent, err := os.ReadFile(imgPath)
if err != nil { if err != nil {
t.Fatalf("unable to load image: %v", err) t.Fatalf("unable to load image: %v", err)
} }

View File

@ -1,7 +1,7 @@
// Code generated by protoc-gen-go. DO NOT EDIT. // Code generated by protoc-gen-go. DO NOT EDIT.
// versions: // versions:
// protoc-gen-go v1.27.1 // protoc-gen-go v1.28.1
// protoc v3.12.4 // protoc v3.21.4
// source: events/events.proto // source: events/events.proto
package events package events
@ -468,17 +468,17 @@ func (x *ObjectsMessage) GetFrameRef() *FrameRef {
return nil return nil
} }
// BoundingBox that contains an object // BoundingBox that contains an object, coordinates as percent
type Object struct { type Object struct {
state protoimpl.MessageState state protoimpl.MessageState
sizeCache protoimpl.SizeCache sizeCache protoimpl.SizeCache
unknownFields protoimpl.UnknownFields unknownFields protoimpl.UnknownFields
Type TypeObject `protobuf:"varint,1,opt,name=type,proto3,enum=robocar.events.TypeObject" json:"type,omitempty"` Type TypeObject `protobuf:"varint,1,opt,name=type,proto3,enum=robocar.events.TypeObject" json:"type,omitempty"`
Left int32 `protobuf:"varint,2,opt,name=left,proto3" json:"left,omitempty"` Left float32 `protobuf:"fixed32,2,opt,name=left,proto3" json:"left,omitempty"`
Top int32 `protobuf:"varint,3,opt,name=top,proto3" json:"top,omitempty"` Top float32 `protobuf:"fixed32,3,opt,name=top,proto3" json:"top,omitempty"`
Right int32 `protobuf:"varint,4,opt,name=right,proto3" json:"right,omitempty"` Right float32 `protobuf:"fixed32,4,opt,name=right,proto3" json:"right,omitempty"`
Bottom int32 `protobuf:"varint,5,opt,name=bottom,proto3" json:"bottom,omitempty"` Bottom float32 `protobuf:"fixed32,5,opt,name=bottom,proto3" json:"bottom,omitempty"`
Confidence float32 `protobuf:"fixed32,6,opt,name=confidence,proto3" json:"confidence,omitempty"` Confidence float32 `protobuf:"fixed32,6,opt,name=confidence,proto3" json:"confidence,omitempty"`
} }
@ -521,28 +521,28 @@ func (x *Object) GetType() TypeObject {
return TypeObject_ANY return TypeObject_ANY
} }
func (x *Object) GetLeft() int32 { func (x *Object) GetLeft() float32 {
if x != nil { if x != nil {
return x.Left return x.Left
} }
return 0 return 0
} }
func (x *Object) GetTop() int32 { func (x *Object) GetTop() float32 {
if x != nil { if x != nil {
return x.Top return x.Top
} }
return 0 return 0
} }
func (x *Object) GetRight() int32 { func (x *Object) GetRight() float32 {
if x != nil { if x != nil {
return x.Right return x.Right
} }
return 0 return 0
} }
func (x *Object) GetBottom() int32 { func (x *Object) GetBottom() float32 {
if x != nil { if x != nil {
return x.Bottom return x.Bottom
} }
@ -809,6 +809,8 @@ type RecordMessage struct {
Frame *FrameMessage `protobuf:"bytes,1,opt,name=frame,proto3" json:"frame,omitempty"` Frame *FrameMessage `protobuf:"bytes,1,opt,name=frame,proto3" json:"frame,omitempty"`
Steering *SteeringMessage `protobuf:"bytes,2,opt,name=steering,proto3" json:"steering,omitempty"` Steering *SteeringMessage `protobuf:"bytes,2,opt,name=steering,proto3" json:"steering,omitempty"`
AutopilotSteering *SteeringMessage `protobuf:"bytes,4,opt,name=autopilot_steering,json=autopilotSteering,proto3" json:"autopilot_steering,omitempty"`
DriveMode *DriveModeMessage `protobuf:"bytes,5,opt,name=drive_mode,json=driveMode,proto3" json:"drive_mode,omitempty"`
RecordSet string `protobuf:"bytes,3,opt,name=recordSet,proto3" json:"recordSet,omitempty"` // Record set name RecordSet string `protobuf:"bytes,3,opt,name=recordSet,proto3" json:"recordSet,omitempty"` // Record set name
} }
@ -858,6 +860,20 @@ func (x *RecordMessage) GetSteering() *SteeringMessage {
return nil return nil
} }
func (x *RecordMessage) GetAutopilotSteering() *SteeringMessage {
if x != nil {
return x.AutopilotSteering
}
return nil
}
func (x *RecordMessage) GetDriveMode() *DriveModeMessage {
if x != nil {
return x.DriveMode
}
return nil
}
func (x *RecordMessage) GetRecordSet() string { func (x *RecordMessage) GetRecordSet() string {
if x != nil { if x != nil {
return x.RecordSet return x.RecordSet
@ -918,10 +934,10 @@ var file_events_events_proto_rawDesc = []byte{
0x0e, 0x32, 0x1a, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x0e, 0x32, 0x1a, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e,
0x74, 0x73, 0x2e, 0x54, 0x79, 0x70, 0x65, 0x4f, 0x62, 0x6a, 0x65, 0x63, 0x74, 0x52, 0x04, 0x74, 0x74, 0x73, 0x2e, 0x54, 0x79, 0x70, 0x65, 0x4f, 0x62, 0x6a, 0x65, 0x63, 0x74, 0x52, 0x04, 0x74,
0x79, 0x70, 0x65, 0x12, 0x12, 0x0a, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x18, 0x02, 0x20, 0x01, 0x28, 0x79, 0x70, 0x65, 0x12, 0x12, 0x0a, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x18, 0x02, 0x20, 0x01, 0x28,
0x05, 0x52, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x12, 0x10, 0x0a, 0x03, 0x74, 0x6f, 0x70, 0x18, 0x03, 0x02, 0x52, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x12, 0x10, 0x0a, 0x03, 0x74, 0x6f, 0x70, 0x18, 0x03,
0x20, 0x01, 0x28, 0x05, 0x52, 0x03, 0x74, 0x6f, 0x70, 0x12, 0x14, 0x0a, 0x05, 0x72, 0x69, 0x67, 0x20, 0x01, 0x28, 0x02, 0x52, 0x03, 0x74, 0x6f, 0x70, 0x12, 0x14, 0x0a, 0x05, 0x72, 0x69, 0x67,
0x68, 0x74, 0x18, 0x04, 0x20, 0x01, 0x28, 0x05, 0x52, 0x05, 0x72, 0x69, 0x67, 0x68, 0x74, 0x12, 0x68, 0x74, 0x18, 0x04, 0x20, 0x01, 0x28, 0x02, 0x52, 0x05, 0x72, 0x69, 0x67, 0x68, 0x74, 0x12,
0x16, 0x0a, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x18, 0x05, 0x20, 0x01, 0x28, 0x05, 0x52, 0x16, 0x0a, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x18, 0x05, 0x20, 0x01, 0x28, 0x02, 0x52,
0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x12, 0x1e, 0x0a, 0x0a, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x12, 0x1e, 0x0a, 0x0a, 0x63, 0x6f, 0x6e, 0x66, 0x69,
0x64, 0x65, 0x6e, 0x63, 0x65, 0x18, 0x06, 0x20, 0x01, 0x28, 0x02, 0x52, 0x0a, 0x63, 0x6f, 0x6e, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x18, 0x06, 0x20, 0x01, 0x28, 0x02, 0x52, 0x0a, 0x63, 0x6f, 0x6e,
0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0x2f, 0x0a, 0x13, 0x53, 0x77, 0x69, 0x74, 0x63, 0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0x2f, 0x0a, 0x13, 0x53, 0x77, 0x69, 0x74, 0x63,
@ -950,7 +966,7 @@ var file_events_events_proto_rawDesc = []byte{
0x74, 0x12, 0x14, 0x0a, 0x05, 0x61, 0x6e, 0x67, 0x6c, 0x65, 0x18, 0x04, 0x20, 0x01, 0x28, 0x02, 0x74, 0x12, 0x14, 0x0a, 0x05, 0x61, 0x6e, 0x67, 0x6c, 0x65, 0x18, 0x04, 0x20, 0x01, 0x28, 0x02,
0x52, 0x05, 0x61, 0x6e, 0x67, 0x6c, 0x65, 0x12, 0x1e, 0x0a, 0x0a, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x52, 0x05, 0x61, 0x6e, 0x67, 0x6c, 0x65, 0x12, 0x1e, 0x0a, 0x0a, 0x63, 0x6f, 0x6e, 0x66, 0x69,
0x64, 0x65, 0x6e, 0x63, 0x65, 0x18, 0x05, 0x20, 0x01, 0x28, 0x02, 0x52, 0x0a, 0x63, 0x6f, 0x6e, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x18, 0x05, 0x20, 0x01, 0x28, 0x02, 0x52, 0x0a, 0x63, 0x6f, 0x6e,
0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0x9e, 0x01, 0x0a, 0x0d, 0x52, 0x65, 0x63, 0x6f, 0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0xaf, 0x02, 0x0a, 0x0d, 0x52, 0x65, 0x63, 0x6f,
0x72, 0x64, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x12, 0x32, 0x0a, 0x05, 0x66, 0x72, 0x61, 0x72, 0x64, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x12, 0x32, 0x0a, 0x05, 0x66, 0x72, 0x61,
0x6d, 0x65, 0x18, 0x01, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x1c, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x6d, 0x65, 0x18, 0x01, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x1c, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63,
0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x2e, 0x46, 0x72, 0x61, 0x6d, 0x65, 0x4d, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x2e, 0x46, 0x72, 0x61, 0x6d, 0x65, 0x4d,
@ -958,16 +974,25 @@ var file_events_events_proto_rawDesc = []byte{
0x08, 0x73, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x18, 0x02, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x08, 0x73, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x18, 0x02, 0x20, 0x01, 0x28, 0x0b, 0x32,
0x1f, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x1f, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73,
0x2e, 0x53, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x2e, 0x53, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65,
0x52, 0x08, 0x73, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x12, 0x1c, 0x0a, 0x09, 0x72, 0x65, 0x52, 0x08, 0x73, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x12, 0x4e, 0x0a, 0x12, 0x61, 0x75,
0x63, 0x6f, 0x72, 0x64, 0x53, 0x65, 0x74, 0x18, 0x03, 0x20, 0x01, 0x28, 0x09, 0x52, 0x09, 0x72, 0x74, 0x6f, 0x70, 0x69, 0x6c, 0x6f, 0x74, 0x5f, 0x73, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67,
0x65, 0x63, 0x6f, 0x72, 0x64, 0x53, 0x65, 0x74, 0x2a, 0x2d, 0x0a, 0x09, 0x44, 0x72, 0x69, 0x76, 0x18, 0x04, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x1f, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72,
0x65, 0x4d, 0x6f, 0x64, 0x65, 0x12, 0x0b, 0x0a, 0x07, 0x49, 0x4e, 0x56, 0x41, 0x4c, 0x49, 0x44, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x2e, 0x53, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67,
0x10, 0x00, 0x12, 0x08, 0x0a, 0x04, 0x55, 0x53, 0x45, 0x52, 0x10, 0x01, 0x12, 0x09, 0x0a, 0x05, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x52, 0x11, 0x61, 0x75, 0x74, 0x6f, 0x70, 0x69, 0x6c,
0x50, 0x49, 0x4c, 0x4f, 0x54, 0x10, 0x02, 0x2a, 0x32, 0x0a, 0x0a, 0x54, 0x79, 0x70, 0x65, 0x4f, 0x6f, 0x74, 0x53, 0x74, 0x65, 0x65, 0x72, 0x69, 0x6e, 0x67, 0x12, 0x3f, 0x0a, 0x0a, 0x64, 0x72,
0x62, 0x6a, 0x65, 0x63, 0x74, 0x12, 0x07, 0x0a, 0x03, 0x41, 0x4e, 0x59, 0x10, 0x00, 0x12, 0x07, 0x69, 0x76, 0x65, 0x5f, 0x6d, 0x6f, 0x64, 0x65, 0x18, 0x05, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x20,
0x0a, 0x03, 0x43, 0x41, 0x52, 0x10, 0x01, 0x12, 0x08, 0x0a, 0x04, 0x42, 0x55, 0x4d, 0x50, 0x10, 0x2e, 0x72, 0x6f, 0x62, 0x6f, 0x63, 0x61, 0x72, 0x2e, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x2e,
0x02, 0x12, 0x08, 0x0a, 0x04, 0x50, 0x4c, 0x4f, 0x54, 0x10, 0x03, 0x42, 0x0a, 0x5a, 0x08, 0x2e, 0x44, 0x72, 0x69, 0x76, 0x65, 0x4d, 0x6f, 0x64, 0x65, 0x4d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65,
0x2f, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x62, 0x06, 0x70, 0x72, 0x6f, 0x74, 0x6f, 0x33, 0x52, 0x09, 0x64, 0x72, 0x69, 0x76, 0x65, 0x4d, 0x6f, 0x64, 0x65, 0x12, 0x1c, 0x0a, 0x09, 0x72,
0x65, 0x63, 0x6f, 0x72, 0x64, 0x53, 0x65, 0x74, 0x18, 0x03, 0x20, 0x01, 0x28, 0x09, 0x52, 0x09,
0x72, 0x65, 0x63, 0x6f, 0x72, 0x64, 0x53, 0x65, 0x74, 0x2a, 0x2d, 0x0a, 0x09, 0x44, 0x72, 0x69,
0x76, 0x65, 0x4d, 0x6f, 0x64, 0x65, 0x12, 0x0b, 0x0a, 0x07, 0x49, 0x4e, 0x56, 0x41, 0x4c, 0x49,
0x44, 0x10, 0x00, 0x12, 0x08, 0x0a, 0x04, 0x55, 0x53, 0x45, 0x52, 0x10, 0x01, 0x12, 0x09, 0x0a,
0x05, 0x50, 0x49, 0x4c, 0x4f, 0x54, 0x10, 0x02, 0x2a, 0x32, 0x0a, 0x0a, 0x54, 0x79, 0x70, 0x65,
0x4f, 0x62, 0x6a, 0x65, 0x63, 0x74, 0x12, 0x07, 0x0a, 0x03, 0x41, 0x4e, 0x59, 0x10, 0x00, 0x12,
0x07, 0x0a, 0x03, 0x43, 0x41, 0x52, 0x10, 0x01, 0x12, 0x08, 0x0a, 0x04, 0x42, 0x55, 0x4d, 0x50,
0x10, 0x02, 0x12, 0x08, 0x0a, 0x04, 0x50, 0x4c, 0x4f, 0x54, 0x10, 0x03, 0x42, 0x0a, 0x5a, 0x08,
0x2e, 0x2f, 0x65, 0x76, 0x65, 0x6e, 0x74, 0x73, 0x62, 0x06, 0x70, 0x72, 0x6f, 0x74, 0x6f, 0x33,
} }
var ( var (
@ -1016,11 +1041,13 @@ var file_events_events_proto_depIdxs = []int32{
11, // 11: robocar.events.Ellipse.center:type_name -> robocar.events.Point 11, // 11: robocar.events.Ellipse.center:type_name -> robocar.events.Point
3, // 12: robocar.events.RecordMessage.frame:type_name -> robocar.events.FrameMessage 3, // 12: robocar.events.RecordMessage.frame:type_name -> robocar.events.FrameMessage
4, // 13: robocar.events.RecordMessage.steering:type_name -> robocar.events.SteeringMessage 4, // 13: robocar.events.RecordMessage.steering:type_name -> robocar.events.SteeringMessage
14, // [14:14] is the sub-list for method output_type 4, // 14: robocar.events.RecordMessage.autopilot_steering:type_name -> robocar.events.SteeringMessage
14, // [14:14] is the sub-list for method input_type 6, // 15: robocar.events.RecordMessage.drive_mode:type_name -> robocar.events.DriveModeMessage
14, // [14:14] is the sub-list for extension type_name 16, // [16:16] is the sub-list for method output_type
14, // [14:14] is the sub-list for extension extendee 16, // [16:16] is the sub-list for method input_type
0, // [0:14] is the sub-list for field type_name 16, // [16:16] is the sub-list for extension type_name
16, // [16:16] is the sub-list for extension extendee
0, // [0:16] is the sub-list for field type_name
} }
func init() { file_events_events_proto_init() } func init() { file_events_events_proto_init() }

2
vendor/modules.txt vendored
View File

@ -3,7 +3,7 @@
github.com/cyrilix/robocar-base/cli github.com/cyrilix/robocar-base/cli
github.com/cyrilix/robocar-base/service github.com/cyrilix/robocar-base/service
github.com/cyrilix/robocar-base/testtools github.com/cyrilix/robocar-base/testtools
# github.com/cyrilix/robocar-protobuf/go v1.0.5 # github.com/cyrilix/robocar-protobuf/go v1.2.0
## explicit; go 1.18 ## explicit; go 1.18
github.com/cyrilix/robocar-protobuf/go/events github.com/cyrilix/robocar-protobuf/go/events
# github.com/eclipse/paho.mqtt.golang v1.4.1 # github.com/eclipse/paho.mqtt.golang v1.4.1