Fix steering/throttle controls

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
+	var steeringMsg events2.SteeringMessage
-	err := proto.Unmarshal(message.Payload(), steeringMsg)
+	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
+	var throttleMsg events2.ThrottleMessage
-	err := proto.Unmarshal(message.Payload(), throttleMsg)
+	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)
 }

pkg/gateway/controls_test.go

@@ -19,37 +19,37 @@ func TestGateway_WriteSteering(t *testing.T) {
 			nil,
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.5,
+				Steering: "0.50",
-				Throttle: 0,
+				Throttle: "0.0",
-				Brake:    0,
+				Brake:    "0.0",
 			}},
 		{"Update steering",
 			&events.SteeringMessage{Steering: -0.5, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0,
+				Throttle: "0.0",
-				Brake:    0,
+				Brake:    "0.0",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: -0.5,
+				Steering: "-0.50",
-				Throttle: 0,
+				Throttle: "0.0",
-				Brake:    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,
+				Steering: "0.2",
-				Throttle: 0.6,
+				Throttle: "0.6",
-				Brake:    0.1,
+				Brake:    "0.1",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: -0.3,
+				Steering: "-0.30",
-				Throttle: 0.6,
+				Throttle: "0.6",
-				Brake:    0.1,
+				Brake:    "0.1",
 			}},
 	}
 
@@ -96,79 +96,79 @@ func TestGateway_WriteThrottle(t *testing.T) {
 			nil,
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0,
+				Steering: "0.0",
-				Throttle: 0.5,
+				Throttle: "0.50",
-				Brake:    0,
+				Brake:    "0.0",
 			}},
 		{"Update Throttle",
 			&events.ThrottleMessage{Throttle: 0.6, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0,
+				Steering: "0",
-				Throttle: 0.4,
+				Throttle: "0.4",
-				Brake:    0,
+				Brake:    "0",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0,
+				Steering: "0",
-				Throttle: 0.6,
+				Throttle: "0.60",
-				Brake:    0,
+				Brake:    "0.0",
 			}},
 		{"Update steering shouldn't erase throttle value",
 			&events.ThrottleMessage{Throttle: 0.3, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.6,
+				Throttle: "0.6",
-				Brake:    0.,
+				Brake:    "0.0",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.3,
+				Throttle: "0.30",
-				Brake:    0.,
+				Brake:    "0.0",
 			}},
 		{"Throttle to brake",
 			&events.ThrottleMessage{Throttle: -0.7, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.6,
+				Throttle: "0.6",
-				Brake:    0.,
+				Brake:    "0.0",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.,
+				Throttle: "0.0",
-				Brake:    0.7,
+				Brake:    "0.70",
 			}},
 		{"Update brake",
 			&events.ThrottleMessage{Throttle: -0.2, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.,
+				Throttle: "0.0",
-				Brake:    0.5,
+				Brake:    "0.5",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.,
+				Throttle: "0.0",
-				Brake:    0.2,
+				Brake:    "0.20",
 			}},
 		{"Brake to throttle",
 			&events.ThrottleMessage{Throttle: 0.9, Confidence: 1},
 			&simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.,
+				Throttle: "0.0",
-				Brake:    0.4,
+				Brake:    "0.4",
 			},
 			simulator.ControlMsg{
 				MsgType:  "control",
-				Steering: 0.2,
+				Steering: "0.2",
-				Throttle: 0.9,
+				Throttle: "0.90",
-				Brake:    0.,
+				Brake:    "0.0",
 			}},
 	}
 

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.Throttle = fmt.Sprintf("%.2f", message.Throttle)
-		g.lastControl.Brake = 0.
+		g.lastControl.Brake = "0.0"
 	} else {
-		g.lastControl.Throttle = 0.
+		g.lastControl.Throttle = "0.0"
-		g.lastControl.Brake = -1 * message.Throttle
+		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.,
+		Steering: "0.0",
-		Throttle: 0.,
+		Throttle: "0.0",
-		Brake:    0.,
+		Brake:    "0.0",
 	}
 }