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

feat: record autopilot steering and driveMode

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

113
pkg/part/record.go
View File

@ -11,12 +11,15 @@ import (
"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{
client: client,
recordTopic: recordTopic,
cameraTopic: cameraTopic,
steeringTopic: steeringTopic,
rcSteeringTopic: rcSteeringTopic,
tfSteeringTopic: tfSteeringTopic,
driveModeTopic: driveModeTopic,
switchRecordTopic: switchRecordTopic,
enabled: false,
idGenerator: NewDateBasedGenerator(),
@ -26,12 +29,20 @@ func NewRecorder(client mqtt.Client, recordTopic, cameraTopic, steeringTopic, sw
}
type Recorder struct {
client mqtt.Client
recordTopic string
cameraTopic, steeringTopic, switchRecordTopic string
client mqtt.Client
recordTopic string
cameraTopic, switchRecordTopic string
muSteeringMsg sync.Mutex
currentSteering *events.SteeringMessage
driveModeTopic, rcSteeringTopic, tfSteeringTopic string
muRcSteeringMsg sync.Mutex
currentRcSteering *events.SteeringMessage
muTfSteeringMsg sync.Mutex
currentTfSteering *events.SteeringMessage
muDriveModeMsg sync.Mutex
currentDriveMode *events.DriveModeMessage
muEnabled sync.RWMutex
enabled bool
@ -57,7 +68,7 @@ func (r *Recorder) Start() error {
func (r *Recorder) Stop() {
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) {
@ -78,7 +89,7 @@ func (r *Recorder) onSwitchRecord(_ mqtt.Client, message mqtt.Message) {
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
err := proto.Unmarshal(message.Payload(), &msg)
if err != nil {
@ -86,9 +97,35 @@ func (r *Recorder) onSteering(_ mqtt.Client, message mqtt.Message) {
return
}
r.muSteeringMsg.Lock()
defer r.muSteeringMsg.Unlock()
r.currentSteering = &msg
r.muRcSteeringMsg.Lock()
defer r.muRcSteeringMsg.Unlock()
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) {
@ -109,10 +146,22 @@ func (r *Recorder) onFrame(_ mqtt.Client, message mqtt.Message) {
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{
Frame: &msg,
Steering: steering,
RecordSet: r.recordSet,
Frame: &msg,
Steering: steering,
AutopilotSteering: autopilot,
DriveMode: driveMode,
RecordSet: r.recordSet,
}
payload, err := proto.Marshal(&record)
@ -128,12 +177,26 @@ var publish = func(client mqtt.Client, topic string, payload *[]byte) {
}
func (r *Recorder) CurrentSteering() *events.SteeringMessage {
r.muSteeringMsg.Lock()
defer r.muSteeringMsg.Unlock()
steering := r.currentSteering
r.muRcSteeringMsg.Lock()
defer r.muRcSteeringMsg.Unlock()
steering := r.currentRcSteering
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 {
r.muEnabled.RLock()
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)
}
err = service.RegisterCallback(r.client, r.steeringTopic, r.onSteering)
err = service.RegisterCallback(r.client, r.rcSteeringTopic, r.onRcSteering)
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)

70
pkg/part/record_test.go
View File

@ -6,7 +6,7 @@ import (
mqtt "github.com/eclipse/paho.mqtt.golang"
"go.uber.org/zap"
"google.golang.org/protobuf/proto"
"io/ioutil"
"os"
"regexp"
"strings"
"sync"
@ -27,6 +27,8 @@ func TestRecorder_Record(t *testing.T) {
recordTopic := "topic/record"
cameraTopic := "topic/camera"
steeringTopic := "topic/steeringMsg"
autopilotSteeringTopic := "topic/autopilot/steering"
driveModeTopic := "topic/driveMode"
switchRecord := "topic/switch/record"
var muEventsPublished sync.Mutex
@ -46,7 +48,13 @@ func TestRecorder_Record(t *testing.T) {
eventsPublished = &msg
}
recorder := NewRecorder(nil, recordTopic, cameraTopic, steeringTopic, switchRecord)
recorder := NewRecorder(nil,
recordTopic,
cameraTopic,
steeringTopic,
autopilotSteeringTopic,
driveModeTopic,
switchRecord)
recorder.idGenerator = &DateBasedGenerator{
muCpt: sync.Mutex{},
cpt: 0,
@ -64,24 +72,52 @@ func TestRecorder_Record(t *testing.T) {
frame2 := loadImage(t, "testdata/img.jpg", "02")
steeringRight := 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 {
recordMsg *events.SwitchRecordMessage
frameMsg *events.FrameMessage
steeringMsg *events.SteeringMessage
autopilotMsg *events.SteeringMessage
driveModeMsg *events.DriveModeMessage
expectedRecordMsg *events.RecordMessage
wait time.Duration
}{
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1, steeringMsg: nil, 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: frame1, steeringMsg: &steeringRight, expectedRecordMsg: &events.RecordMessage{RecordSet: "record-1", Frame: frame1, Steering: &steeringRight}, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil, steeringMsg: &steeringLeft, 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: false}, frameMsg: nil, steeringMsg: nil, expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: false}, frameMsg: nil, steeringMsg: nil, 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: false}, frameMsg: nil,
steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: nil,
steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, wait: 5 * time.Millisecond},
{recordMsg: &events.SwitchRecordMessage{Enabled: true}, frameMsg: frame1,
steeringMsg: nil, autopilotMsg: nil, driveModeMsg: nil,
expectedRecordMsg: nil, 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 {
@ -89,6 +125,12 @@ func TestRecorder_Record(t *testing.T) {
eventsPublished = nil
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 {
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))
}
if c.steeringMsg != nil {
recorder.onSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, c.steeringMsg))
recorder.onRcSteering(nil, testtools.NewFakeMessageFromProtobuf(steeringTopic, c.steeringMsg))
}
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 {
jpegContent, err := ioutil.ReadFile(imgPath)
jpegContent, err := os.ReadFile(imgPath)
if err != nil {
t.Fatalf("unable to load image: %v", err)
}