diff --git a/cmd/rc-steering/rc-steering.go b/cmd/rc-steering/rc-steering.go index 3af67ae..f11d84a 100644 --- a/cmd/rc-steering/rc-steering.go +++ b/cmd/rc-steering/rc-steering.go @@ -15,8 +15,7 @@ const ( func main() { var mqttBroker, username, password, clientId string - var steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic string - var debug bool + var steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string mqttQos := cli.InitIntFlag("MQTT_QOS", 0) _, 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(&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(&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") @@ -50,14 +50,13 @@ func main() { }() zap.ReplaceGlobals(lgr) - debug = logLevel.Enabled(zap.DebugLevel) client, err := cli.Connect(mqttBroker, username, password, clientId) if err != nil { log.Fatalf("unable to connect to mqtt bus: %v", err) } 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() cli.HandleExit(p) diff --git a/go.mod b/go.mod index 5447a28..63ade35 100644 --- a/go.mod +++ b/go.mod @@ -4,7 +4,7 @@ go 1.18 require ( 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 go.uber.org/zap v1.21.0 gocv.io/x/gocv v0.31.0 diff --git a/go.sum b/go.sum index 60f231d..fc8735a 100644 --- a/go.sum +++ b/go.sum @@ -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-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.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.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c= github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= diff --git a/pkg/steering/controller.go b/pkg/steering/controller.go index ba465ec..66e8acc 100644 --- a/pkg/steering/controller.go +++ b/pkg/steering/controller.go @@ -9,13 +9,14 @@ import ( "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{ client: client, steeringTopic: steeringTopic, driveModeTopic: driveModeTopic, rcSteeringTopic: rcSteeringTopic, tfSteeringTopic: tfSteeringTopic, + objectsTopic: objectsTopic, driveMode: events.DriveMode_USER, } @@ -28,8 +29,11 @@ type Controller struct { muDriveMode sync.RWMutex driveMode events.DriveMode - cancel chan interface{} - driveModeTopic, rcSteeringTopic, tfSteeringTopic string + cancel chan interface{} + driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string + + muObjects sync.RWMutex + objects []*events.Object debug bool } @@ -50,6 +54,19 @@ func (p *Controller) Stop() { 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) { var msg events.DriveModeMessage err := proto.Unmarshal(message.Payload(), &msg) @@ -115,6 +132,11 @@ var registerCallbacks = func(p *Controller) error { if err != nil { return err } + + err = service.RegisterCallback(p.client, p.objectsTopic, p.onObjects) + if err != nil { + return err + } return nil } diff --git a/pkg/steering/controller_test.go b/pkg/steering/controller_test.go index f1bc38b..a7c4aec 100644 --- a/pkg/steering/controller_test.go +++ b/pkg/steering/controller_test.go @@ -33,50 +33,58 @@ func TestDefaultSteering(t *testing.T) { driveModeTopic := "topic/driveMode" rcSteeringTopic := "topic/rcSteering" tfSteeringTopic := "topic/tfSteering" + objectsTopic := "topic/objects" - p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, true) + p := NewController(nil, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic) cases := []struct { driveMode events.DriveModeMessage rcSteering events.SteeringMessage tfSteering events.SteeringMessage expectedSteering events.SteeringMessage + objects events.ObjectsMessage }{ { events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.SteeringMessage{Steering: 0.3, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.3, Confidence: 1.0}, + events.ObjectsMessage{}, }, { events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, 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.ObjectsMessage{}, }, { events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, 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.ObjectsMessage{}, }, { events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.SteeringMessage{Steering: 0.5, Confidence: 1.0}, events.SteeringMessage{Steering: 0.8, Confidence: 1.0}, events.SteeringMessage{Steering: 0.5, Confidence: 1.0}, + events.ObjectsMessage{}, }, { events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, events.SteeringMessage{Steering: 0.9, Confidence: 1.0}, events.SteeringMessage{Steering: 0.4, Confidence: 1.0}, + events.ObjectsMessage{}, }, { events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.SteeringMessage{Steering: 0.6, Confidence: 1.0}, events.SteeringMessage{Steering: -0.3, 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.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &c.rcSteering)) p.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &c.tfSteering)) + p.onObjects(nil, testtools.NewFakeMessageFromProtobuf(objectsTopic, &c.objects)) time.Sleep(10 * time.Millisecond) diff --git a/vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go b/vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go index 7e53812..f1dc655 100644 --- a/vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go +++ b/vendor/github.com/cyrilix/robocar-protobuf/go/events/events.pb.go @@ -1,7 +1,7 @@ // Code generated by protoc-gen-go. DO NOT EDIT. // versions: -// protoc-gen-go v1.27.1 -// protoc v3.12.4 +// protoc-gen-go v1.28.1 +// protoc v3.21.4 // source: events/events.proto package events @@ -468,17 +468,17 @@ func (x *ObjectsMessage) GetFrameRef() *FrameRef { return nil } -// BoundingBox that contains an object +// BoundingBox that contains an object, coordinates as percent type Object struct { state protoimpl.MessageState sizeCache protoimpl.SizeCache unknownFields protoimpl.UnknownFields 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"` - Top int32 `protobuf:"varint,3,opt,name=top,proto3" json:"top,omitempty"` - Right int32 `protobuf:"varint,4,opt,name=right,proto3" json:"right,omitempty"` - Bottom int32 `protobuf:"varint,5,opt,name=bottom,proto3" json:"bottom,omitempty"` + Left float32 `protobuf:"fixed32,2,opt,name=left,proto3" json:"left,omitempty"` + Top float32 `protobuf:"fixed32,3,opt,name=top,proto3" json:"top,omitempty"` + Right float32 `protobuf:"fixed32,4,opt,name=right,proto3" json:"right,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"` } @@ -521,28 +521,28 @@ func (x *Object) GetType() TypeObject { return TypeObject_ANY } -func (x *Object) GetLeft() int32 { +func (x *Object) GetLeft() float32 { if x != nil { return x.Left } return 0 } -func (x *Object) GetTop() int32 { +func (x *Object) GetTop() float32 { if x != nil { return x.Top } return 0 } -func (x *Object) GetRight() int32 { +func (x *Object) GetRight() float32 { if x != nil { return x.Right } return 0 } -func (x *Object) GetBottom() int32 { +func (x *Object) GetBottom() float32 { if x != nil { 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, 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, - 0x05, 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, - 0x68, 0x74, 0x18, 0x04, 0x20, 0x01, 0x28, 0x05, 0x52, 0x05, 0x72, 0x69, 0x67, 0x68, 0x74, 0x12, - 0x16, 0x0a, 0x06, 0x62, 0x6f, 0x74, 0x74, 0x6f, 0x6d, 0x18, 0x05, 0x20, 0x01, 0x28, 0x05, 0x52, + 0x02, 0x52, 0x04, 0x6c, 0x65, 0x66, 0x74, 0x12, 0x10, 0x0a, 0x03, 0x74, 0x6f, 0x70, 0x18, 0x03, + 0x20, 0x01, 0x28, 0x02, 0x52, 0x03, 0x74, 0x6f, 0x70, 0x12, 0x14, 0x0a, 0x05, 0x72, 0x69, 0x67, + 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, 0x02, 0x52, 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, 0x66, 0x69, 0x64, 0x65, 0x6e, 0x63, 0x65, 0x22, 0x2f, 0x0a, 0x13, 0x53, 0x77, 0x69, 0x74, 0x63, diff --git a/vendor/modules.txt b/vendor/modules.txt index 3f8ddb3..5c0911d 100644 --- a/vendor/modules.txt +++ b/vendor/modules.txt @@ -3,7 +3,7 @@ github.com/cyrilix/robocar-base/cli github.com/cyrilix/robocar-base/service 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 github.com/cyrilix/robocar-protobuf/go/events # github.com/eclipse/paho.mqtt.golang v1.4.1