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

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