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

Fix steering/throttle controls

Browse Source
This commit is contained in:
Cyrille Nofficial
2021-02-08 23:07:09 +01:00
parent 5a99160f65
commit d0c8ef76fc
3 changed files with 72 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
cmd/rc-simulator/rc-simultator.go
View File

@@ -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)
}

96
pkg/gateway/controls_test.go
View File

@@ -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",
}},
}

17
pkg/gateway/gateway.go
View File

@@ -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",
}
}