diff --git a/cmd/rc-simulator/rc-simultator.go b/cmd/rc-simulator/rc-simultator.go index 589952d..323eac6 100644 --- a/cmd/rc-simulator/rc-simultator.go +++ b/cmd/rc-simulator/rc-simultator.go @@ -51,6 +51,7 @@ func main() { gtw := gateway.New(address) defer gtw.Stop() + msgPub := events.NewMsgPublisher( gtw, events.NewMqttPublisher(client), @@ -63,42 +64,42 @@ func main() { cli.HandleExit(gtw) - err = gtw.Start() - if err != nil { - log.Fatalf("unable to start service: %v", err) - } - - if topicCtrlSteering != "" { log.Infof("configure mqtt route on steering command") - client.AddRoute(topicCtrlSteering, func(client mqtt.Client, message mqtt.Message) { + client.Subscribe(topicCtrlSteering, byte(mqttQos), func(client mqtt.Client, message mqtt.Message) { onSteeringCommand(gtw, message) }) } if topicCtrlThrottle != "" { log.Infof("configure mqtt route on throttle command") - client.AddRoute(topicCtrlThrottle, func(client mqtt.Client, message mqtt.Message) { + client.Subscribe(topicCtrlThrottle, byte(mqttQos), func(client mqtt.Client, message mqtt.Message) { onThrottleCommand(gtw, message) }) } + + err = gtw.Start() + if err != nil { + log.Fatalf("unable to start service: %v", err) + } + } func onSteeringCommand(c *gateway.Gateway, message mqtt.Message) { - var steeringMsg *events2.SteeringMessage - err := proto.Unmarshal(message.Payload(), steeringMsg) + var steeringMsg events2.SteeringMessage + err := proto.Unmarshal(message.Payload(), &steeringMsg) if err != nil { log.Errorf("unable to unmarshal steering msg: %v", err) return } - c.WriteSteering(steeringMsg) + c.WriteSteering(&steeringMsg) } func onThrottleCommand(c *gateway.Gateway, message mqtt.Message) { - var throttleMsg *events2.ThrottleMessage - err := proto.Unmarshal(message.Payload(), throttleMsg) + var throttleMsg events2.ThrottleMessage + err := proto.Unmarshal(message.Payload(), &throttleMsg) if err != nil { log.Errorf("unable to unmarshal throttle msg: %v", err) return } - c.WriteThrottle(throttleMsg) + c.WriteThrottle(&throttleMsg) } diff --git a/pkg/gateway/controls_test.go b/pkg/gateway/controls_test.go index a865f6b..6d09710 100644 --- a/pkg/gateway/controls_test.go +++ b/pkg/gateway/controls_test.go @@ -19,37 +19,37 @@ func TestGateway_WriteSteering(t *testing.T) { nil, simulator.ControlMsg{ MsgType: "control", - Steering: 0.5, - Throttle: 0, - Brake: 0, + Steering: "0.50", + Throttle: "0.0", + Brake: "0.0", }}, {"Update steering", &events.SteeringMessage{Steering: -0.5, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0, - Brake: 0, + Steering: "0.2", + Throttle: "0.0", + Brake: "0.0", }, simulator.ControlMsg{ MsgType: "control", - Steering: -0.5, - Throttle: 0, - Brake: 0, + Steering: "-0.50", + Throttle: "0.0", + Brake: "0.0", }}, {"Update steering shouldn't erase throttle value", &events.SteeringMessage{Steering: -0.3, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0.6, - Brake: 0.1, + Steering: "0.2", + Throttle: "0.6", + Brake: "0.1", }, simulator.ControlMsg{ MsgType: "control", - Steering: -0.3, - Throttle: 0.6, - Brake: 0.1, + Steering: "-0.30", + Throttle: "0.6", + Brake: "0.1", }}, } @@ -96,79 +96,79 @@ func TestGateway_WriteThrottle(t *testing.T) { nil, simulator.ControlMsg{ MsgType: "control", - Steering: 0, - Throttle: 0.5, - Brake: 0, + Steering: "0.0", + Throttle: "0.50", + Brake: "0.0", }}, {"Update Throttle", &events.ThrottleMessage{Throttle: 0.6, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0, - Throttle: 0.4, - Brake: 0, + Steering: "0", + Throttle: "0.4", + Brake: "0", }, simulator.ControlMsg{ MsgType: "control", - Steering: 0, - Throttle: 0.6, - Brake: 0, + Steering: "0", + Throttle: "0.60", + Brake: "0.0", }}, {"Update steering shouldn't erase throttle value", &events.ThrottleMessage{Throttle: 0.3, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0.6, - Brake: 0., + Steering: "0.2", + Throttle: "0.6", + Brake: "0.0", }, simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0.3, - Brake: 0., + Steering: "0.2", + Throttle: "0.30", + Brake: "0.0", }}, {"Throttle to brake", &events.ThrottleMessage{Throttle: -0.7, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0.6, - Brake: 0., + Steering: "0.2", + Throttle: "0.6", + Brake: "0.0", }, simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0., - Brake: 0.7, + Steering: "0.2", + Throttle: "0.0", + Brake: "0.70", }}, {"Update brake", &events.ThrottleMessage{Throttle: -0.2, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0., - Brake: 0.5, + Steering: "0.2", + Throttle: "0.0", + Brake: "0.5", }, simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0., - Brake: 0.2, + Steering: "0.2", + Throttle: "0.0", + Brake: "0.20", }}, {"Brake to throttle", &events.ThrottleMessage{Throttle: 0.9, Confidence: 1}, &simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0., - Brake: 0.4, + Steering: "0.2", + Throttle: "0.0", + Brake: "0.4", }, simulator.ControlMsg{ MsgType: "control", - Steering: 0.2, - Throttle: 0.9, - Brake: 0., + Steering: "0.2", + Throttle: "0.90", + Brake: "0.0", }}, } diff --git a/pkg/gateway/gateway.go b/pkg/gateway/gateway.go index d6a248f..6c1cb9b 100644 --- a/pkg/gateway/gateway.go +++ b/pkg/gateway/gateway.go @@ -253,7 +253,7 @@ func (g *Gateway) WriteSteering(message *events.SteeringMessage) { defer g.muControl.Unlock() g.initLastControlMsg() - g.lastControl.Steering = message.Steering + g.lastControl.Steering = fmt.Sprintf("%.2f", message.Steering) g.writeControlCommandToSimulator() } @@ -262,6 +262,7 @@ func (g *Gateway) writeControlCommandToSimulator() { g.log.Errorf("unable to connect to simulator to send control command: %v", err) return } + log.Debugf("write command to simulator: %v", g.lastControl) w := bufio.NewWriter(g.conn) content, err := json.Marshal(g.lastControl) if err != nil { @@ -287,11 +288,11 @@ func (g *Gateway) WriteThrottle(message *events.ThrottleMessage) { g.initLastControlMsg() if message.Throttle > 0 { - g.lastControl.Throttle = message.Throttle - g.lastControl.Brake = 0. + g.lastControl.Throttle = fmt.Sprintf("%.2f", message.Throttle) + g.lastControl.Brake = "0.0" } else { - g.lastControl.Throttle = 0. - g.lastControl.Brake = -1 * message.Throttle + g.lastControl.Throttle = "0.0" + g.lastControl.Brake = fmt.Sprintf("%.2f", -1 * message.Throttle) } g.writeControlCommandToSimulator() @@ -303,8 +304,8 @@ func (g *Gateway) initLastControlMsg() { } g.lastControl = &simulator.ControlMsg{ MsgType: "control", - Steering: 0., - Throttle: 0., - Brake: 0., + Steering: "0.0", + Throttle: "0.0", + Brake: "0.0", } }