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

Consume objects from mqtt topic

Browse Source
This commit is contained in:
Cyrille Nofficial 2022-08-22 13:12:12 +02:00
parent 2b31a3b7eb
commit 9af8f3a770
7 changed files with 57 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cmd/rc-steering/rc-steering.go
View File

@ -15,8 +15,7 @@ const (
func main() { func main() {
var mqttBroker, username, password, clientId string var mqttBroker, username, password, clientId string
var steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string var steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string
var debug bool
mqttQos := cli.InitIntFlag("MQTT_QOS", 0) mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
_, mqttRetain := os.LookupEnv("MQTT_RETAIN") _, mqttRetain := os.LookupEnv("MQTT_RETAIN")
@ -27,6 +26,7 @@ func main() {
flag.StringVar(&rcSteeringTopic, "mqtt-topic-rc-steering", os.Getenv("MQTT_TOPIC_RC_STEERING"), "Mqtt topic that contains RC steering value, use MQTT_TOPIC_RC_STEERING if args not set") flag.StringVar(&rcSteeringTopic, "mqtt-topic-rc-steering", os.Getenv("MQTT_TOPIC_RC_STEERING"), "Mqtt topic that contains RC steering value, use MQTT_TOPIC_RC_STEERING if args not set")
flag.StringVar(&tfSteeringTopic, "mqtt-topic-tf-steering", os.Getenv("MQTT_TOPIC_TF_STEERING"), "Mqtt topic that contains tenorflow steering value, use MQTT_TOPIC_TF_STEERING if args not set") flag.StringVar(&tfSteeringTopic, "mqtt-topic-tf-steering", os.Getenv("MQTT_TOPIC_TF_STEERING"), "Mqtt topic that contains tenorflow steering value, use MQTT_TOPIC_TF_STEERING if args not set")
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(&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(&objectsTopic, "mqtt-topic-objects", os.Getenv("MQTT_TOPIC_OBJECTS"), "Mqtt topic that contains Objects from object detection value, use MQTT_TOPIC_OBJECTS if args not set")
logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level") logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
@ -50,14 +50,13 @@ func main() {
}() }()
zap.ReplaceGlobals(lgr) zap.ReplaceGlobals(lgr)
debug = logLevel.Enabled(zap.DebugLevel)
client, err := cli.Connect(mqttBroker, username, password, clientId) client, err := cli.Connect(mqttBroker, username, password, clientId)
if err != nil { if err != nil {
log.Fatalf("unable to connect to mqtt bus: %v", err) log.Fatalf("unable to connect to mqtt bus: %v", err)
} }
defer client.Disconnect(50) defer client.Disconnect(50)
p := steering.NewController(client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, debug) p := steering.NewController(client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic)
defer p.Stop() defer p.Stop()
cli.HandleExit(p) cli.HandleExit(p)

2
go.mod
View File

@ -4,7 +4,7 @@ go 1.18
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.1.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
gocv.io/x/gocv v0.31.0 gocv.io/x/gocv v0.31.0

2
go.sum
View File

@ -4,6 +4,8 @@ github.com/cyrilix/robocar-base v0.1.7 h1:EVzZ0KjigSFpke5f3A/PybEH3WFUEIrYSc3z/d
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.0.5 h1:PX1At+pf6G7gJwT4LzJLQu3/LPFTTNNlZmZSYtnSELY=
github.com/cyrilix/robocar-protobuf/go v1.0.5/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ= github.com/cyrilix/robocar-protobuf/go v1.0.5/go.mod h1:Y3AE28K5V7EZxMXp/6A8RhkRz15VOfFy4CjST35FbtQ=
github.com/cyrilix/robocar-protobuf/go v1.1.0 h1:txIjGnnCF3UzedpsWu+sL7nMA+pNjSnX6HZlAmuReH4=
github.com/cyrilix/robocar-protobuf/go v1.1.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=

28
pkg/steering/controller.go
View File

@ -9,13 +9,14 @@ import (
"sync" "sync"
) )
func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string, debug bool) *Controller { func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string) *Controller {
return &Controller{ return &Controller{
client: client, client: client,
steeringTopic: steeringTopic, steeringTopic: steeringTopic,
driveModeTopic: driveModeTopic, driveModeTopic: driveModeTopic,
rcSteeringTopic: rcSteeringTopic, rcSteeringTopic: rcSteeringTopic,
tfSteeringTopic: tfSteeringTopic, tfSteeringTopic: tfSteeringTopic,
objectsTopic: objectsTopic,
driveMode: events.DriveMode_USER, driveMode: events.DriveMode_USER,
} }
@ -28,8 +29,11 @@ type Controller struct {
muDriveMode sync.RWMutex muDriveMode sync.RWMutex
driveMode events.DriveMode driveMode events.DriveMode
cancel chan interface{} cancel chan interface{}
driveModeTopic, rcSteeringTopic, tfSteeringTopic string driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string
muObjects sync.RWMutex
objects []*events.Object
debug bool debug bool
} }
@ -50,6 +54,19 @@ func (p *Controller) Stop() {
service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic) service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic)
} }
func (p *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
var msg events.ObjectsMessage
err := proto.Unmarshal(message.Payload(), &msg)
if err != nil {
zap.S().Errorf("unable to unmarshal protobuf %T message: %v", msg, err)
return
}
p.muObjects.Lock()
defer p.muObjects.Unlock()
p.objects = msg.GetObjects()
}
func (p *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) { func (p *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
var msg events.DriveModeMessage var msg events.DriveModeMessage
err := proto.Unmarshal(message.Payload(), &msg) err := proto.Unmarshal(message.Payload(), &msg)
@ -115,6 +132,11 @@ var registerCallbacks = func(p *Controller) error {
if err != nil { if err != nil {
return err return err
} }
err = service.RegisterCallback(p.client, p.objectsTopic, p.onObjects)
if err != nil {
return err
}
return nil return nil
} }

11
pkg/steering/controller_test.go
View File

@ -33,50 +33,58 @@ func TestDefaultSteering(t *testing.T) {
driveModeTopic := "topic/driveMode" driveModeTopic := "topic/driveMode"
rcSteeringTopic := "topic/rcSteering" rcSteeringTopic := "topic/rcSteering"
tfSteeringTopic := "topic/tfSteering" tfSteeringTopic := "topic/tfSteering"
objectsTopic := "topic/objects"
p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, true) p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic)
cases := []struct { cases := []struct {
driveMode events.DriveModeMessage driveMode events.DriveModeMessage
rcSteering events.SteeringMessage rcSteering events.SteeringMessage
tfSteering events.SteeringMessage tfSteering events.SteeringMessage
expectedSteering events.SteeringMessage expectedSteering events.SteeringMessage
objects events.ObjectsMessage
}{ }{
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.3, Confidence: 1.0}, events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.3, Confidence: 1.0}, events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
events.ObjectsMessage{},
}, },
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0}, events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0}, events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0}, events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.ObjectsMessage{},
}, },
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.7, Confidence: 1.0}, events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
events.SteeringMessage{Steering: 0.7, Confidence: 1.0}, events.SteeringMessage{Steering: 0.7, Confidence: 1.0},
events.ObjectsMessage{},
}, },
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0}, events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
events.SteeringMessage{Steering: 0.8, Confidence: 1.0}, events.SteeringMessage{Steering: 0.8, Confidence: 1.0},
events.SteeringMessage{Steering: 0.5, Confidence: 1.0}, events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
events.ObjectsMessage{},
}, },
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.SteeringMessage{Steering: 0.9, Confidence: 1.0}, events.SteeringMessage{Steering: 0.9, Confidence: 1.0},
events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
events.ObjectsMessage{},
}, },
{ {
events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0}, events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.SteeringMessage{Steering: -0.3, Confidence: 1.0}, events.SteeringMessage{Steering: -0.3, Confidence: 1.0},
events.SteeringMessage{Steering: 0.6, Confidence: 1.0}, events.SteeringMessage{Steering: 0.6, Confidence: 1.0},
events.ObjectsMessage{},
}, },
} }
@ -88,6 +96,7 @@ func TestDefaultSteering(t *testing.T) {
p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode)) p.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &c.driveMode))
p.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &c.rcSteering)) p.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &c.rcSteering))
p.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &c.tfSteering)) p.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &c.tfSteering))
p.onObjects(nil, testtools.NewFakeMessageFromProtobuf(objectsTopic, &c.objects))
time.Sleep(10 * time.Millisecond) time.Sleep(10 * time.Millisecond)

30
vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go generated vendored
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
} }
@ -918,10 +918,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,

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.1.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