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

Refactor use protbof to send messages and remove unused "distance" metrics

Browse Source
This commit is contained in:
Cyrille Nofficial
2020-01-01 17:12:18 +01:00
parent 2898bbfb95
commit 8a93e9f030
98 changed files with 33878 additions and 418 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
arduino/arduino.go
View File

@@ -2,11 +2,12 @@ package arduino
import (
"bufio"
"github.com/cyrilix/robocar-base/mqttdevice"
"github.com/cyrilix/robocar-base/types"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"github.com/golang/protobuf/proto"
log "github.com/sirupsen/logrus"
"github.com/tarm/serial"
"io"
"log"
"regexp"
"strconv"
"strings"
@@ -27,26 +28,34 @@ var (
)
type Part struct {
pub mqttdevice.Publisher
client mqtt.Client
topicBase string
pubFrequency float64
serial io.Reader
mutex sync.Mutex
steering float64
throttle float64
distanceCm int
steering float32
throttle float32
ctrlRecord bool
driveMode types.DriveMode
driveMode events.DriveMode
debug bool
cancel chan interface{}
}
func NewPart(name string, baud int, pub mqttdevice.Publisher, topicBase string, pubFrequency float64, debug bool) *Part {
func NewPart(client mqtt.Client, name string, baud int, topicBase string, pubFrequency float64, debug bool) *Part {
c := &serial.Config{Name: name, Baud: baud}
s, err := serial.OpenPort(c)
if err != nil {
log.Panicf("unable to open serial port: %v", err)
}
return &Part{serial: s, pub: pub, topicBase: topicBase, pubFrequency: pubFrequency, driveMode: types.DriveModeInvalid, debug: debug}
return &Part{
client: client,
serial: s,
topicBase: topicBase,
pubFrequency: pubFrequency,
driveMode: events.DriveMode_INVALID,
debug: debug,
cancel: make(chan interface{}),
}
}
func (a *Part) Start() error {
@@ -80,11 +89,11 @@ func (a *Part) updateValues(values []string) {
a.processChannel4(values[4])
a.processChannel5(values[5])
a.processChannel6(values[6])
a.processDistanceCm(values[8])
}
func (a *Part) Stop() {
log.Printf("stop ArduinoPart")
close(a.cancel)
switch s := a.serial.(type) {
case io.ReadCloser:
if err := s.Close(); err != nil {
@@ -106,7 +115,7 @@ func (a *Part) processChannel1(v string) {
} else if value > MaxPwmAngle {
value = MaxPwmAngle
}
a.steering = ((float64(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
a.steering = ((float32(value)-MinPwmAngle)/(MaxPwmAngle-MinPwmAngle))*2.0 - 1.0
}
func (a *Part) processChannel2(v string) {
@@ -122,7 +131,7 @@ func (a *Part) processChannel2(v string) {
} else if value > MaxPwmThrottle {
value = MaxPwmThrottle
}
a.throttle = ((float64(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
a.throttle = ((float32(value)-MinPwmThrottle)/(MaxPwmThrottle-MinPwmThrottle))*2.0 - 1.0
}
func (a *Part) processChannel3(v string) {
@@ -170,41 +179,93 @@ func (a *Part) processChannel6(v string) {
return
}
if value > 1800 {
if a.driveMode != types.DriveModePilot {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, types.DriveModeUser)
a.driveMode = types.DriveModePilot
if a.driveMode != events.DriveMode_PILOT {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, events.DriveMode_PILOT)
a.driveMode = events.DriveMode_PILOT
}
} else {
if a.driveMode != types.DriveModeUser {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, types.DriveModeUser)
if a.driveMode != events.DriveMode_USER {
log.Printf("Update channel 6 with value %v, new user_mode: %v", value, events.DriveMode_USER)
}
a.driveMode = types.DriveModeUser
a.driveMode = events.DriveMode_USER
}
}
func (a *Part) processDistanceCm(v string) {
value, err := strconv.Atoi(v)
if err != nil {
log.Printf("invalid value for distanceCm, should be an int: %v", err)
return
}
a.distanceCm = value
}
func (a *Part) publishLoop() {
prefix := strings.TrimSuffix(a.topicBase, "/")
ticker := time.NewTicker(time.Second / time.Duration(int(a.pubFrequency)))
for {
a.publishValues(prefix)
time.Sleep(time.Second / time.Duration(int(a.pubFrequency)))
select {
case <-ticker.C:
a.publishValues(prefix)
case <-a.cancel:
ticker.Stop()
return
}
}
}
func (a *Part) publishValues(prefix string) {
a.mutex.Lock()
defer a.mutex.Unlock()
a.pub.Publish(prefix+"/throttle", mqttdevice.NewMqttValue(types.Throttle{Value: a.throttle, Confidence: 1.}))
a.pub.Publish(prefix+"/steering", mqttdevice.NewMqttValue(types.Steering{Value: a.steering, Confidence: 1.}))
a.pub.Publish(prefix+"/drive_mode", mqttdevice.NewMqttValue(types.ToString(a.driveMode)))
a.pub.Publish(prefix+"/switch_record", mqttdevice.NewMqttValue(a.ctrlRecord))
a.pub.Publish(prefix+"/distance_cm", mqttdevice.NewMqttValue(a.distanceCm))
a.publishThrottle(prefix)
a.publishSteering(prefix)
a.publishDriveMode(prefix)
a.publishSwitchRecord(prefix)
}
func (a *Part) publishThrottle(prefix string) {
throttle := events.ThrottleMessage{
Throttle: a.throttle,
Confidence: 1.0,
}
throttleMessage, err := proto.Marshal(&throttle)
if err != nil {
log.Errorf("unable to marshal protobuf throttle message: %v", err)
return
}
publish(a.client, prefix+"/throttle", &throttleMessage)
}
func (a *Part) publishSteering(prefix string) {
steering := events.SteeringMessage{
Steering: a.steering,
Confidence: 1.0,
}
steeringMessage, err := proto.Marshal(&steering)
if err != nil {
log.Errorf("unable to marshal protobuf steering message: %v", err)
return
}
publish(a.client, prefix+"/steering", &steeringMessage)
}
func (a *Part) publishDriveMode(prefix string) {
dm := events.DriveModeMessage{
DriveMode: a.driveMode,
}
driveModeMessage, err := proto.Marshal(&dm)
if err != nil {
log.Errorf("unable to marshal protobuf driveMode message: %v", err)
return
}
publish(a.client, prefix+"/drive_mode", &driveModeMessage)
}
func (a *Part) publishSwitchRecord(prefix string) {
sr := events.SwitchRecordMessage{
Enabled: a.ctrlRecord,
}
switchRecordMessage, err := proto.Marshal(&sr)
if err != nil {
log.Errorf("unable to marshal protobuf SwitchRecord message: %v", err)
return
}
publish(a.client, prefix+"/switch_record", &switchRecordMessage)
}
var publish = func(client mqtt.Client, topic string, payload *[]byte) {
client.Publish(topic, 0, false, *payload)
}

209
arduino/arduino_test.go
View File

@@ -3,121 +3,137 @@ package arduino
import (
"bufio"
"fmt"
"github.com/cyrilix/robocar-base/mqttdevice"
"github.com/cyrilix/robocar-base/testtools"
"github.com/cyrilix/robocar-base/types"
"github.com/cyrilix/robocar-protobuf/go/events"
mqtt "github.com/eclipse/paho.mqtt.golang"
"github.com/golang/protobuf/proto"
"net"
"sync"
"testing"
"time"
)
func TestArduinoPart_Update(t *testing.T) {
oldPublish := publish
defer func() { publish = oldPublish }()
publish = func(client mqtt.Client, topic string, payload *[]byte) {}
ln, err := net.Listen("tcp", ":8080")
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer ln.Close()
defer func() {
if err := ln.Close(); err != nil {
t.Errorf("unable to close resource: %v", err)
}
}()
client, err := net.Dial("tcp", "localhost:8080")
serialClient, err := net.Dial("tcp", "localhost:8080")
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer client.Close()
defer func() {
if err := serialClient.Close(); err != nil {
t.Errorf("unable to close resource: %v", err)
}
}()
conn, err := ln.Accept()
if err != nil {
t.Fatalf("unable to init connection for test")
}
defer conn.Close()
defer func() {
if err := conn.Close(); err != nil {
t.Errorf("unable to close resource: %v", err)
}
}()
a := Part{serial: conn, pubFrequency: 100, pub: testtools.NewFakePublisher()}
go a.Start()
a := Part{client: nil, serial: conn, pubFrequency: 100}
go func() {
err := a.Start()
if err != nil {
t.Errorf("unsable to start part: %v", err)
t.Fail()
}
}()
channel1, channel2, channel3, channel4, channel5, channel6, distanceCm := 678, 910, 1112, 1678, 1910, 112, 128
cases := []struct {
name, content string
expectedThrottle, expectedSteering float32
expectedDriveMode types.DriveMode
expectedDriveMode events.DriveMode
expectedSwitchRecord bool
expectedDistanceCm int
}{
{"Good value",
fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Unparsable line",
"12350,invalid line\n",
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Switch record on",
fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 998, channel6, distanceCm),
-1., -1., types.DriveModeUser, true, distanceCm},
-1., -1., events.DriveMode_USER, true},
{"Switch record off",
fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1987, channel6, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Switch record off",
fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1850, channel6, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Switch record on",
fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, 1003, channel6, distanceCm),
-1., -1., types.DriveModeUser, true, distanceCm},
-1., -1., events.DriveMode_USER, true},
{"DriveMode: user",
fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 998, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"DriveMode: pilot",
fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1987, distanceCm),
-1., -1., types.DriveModePilot, false, distanceCm},
-1., -1., events.DriveMode_PILOT, false},
{"DriveMode: pilot",
fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1850, distanceCm),
-1., -1., types.DriveModePilot, false, distanceCm},
-1., -1., events.DriveMode_PILOT, false},
// DriveMode: user
{"DriveMode: user",
fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, 1003, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Sterring: over left",
fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,50,%d\n", 99, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Sterring: left",
fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,50,%d\n", 998, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.93, types.DriveModeUser, false, distanceCm},
-1., -0.93, events.DriveMode_USER, false},
{"Sterring: middle",
fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,50,%d\n", 1450, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., -0.04, types.DriveModeUser, false, distanceCm},
-1., -0.04, events.DriveMode_USER, false},
{"Sterring: right",
fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,50,%d\n", 1958, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., 0.96, types.DriveModeUser, false, distanceCm},
-1., 0.96, events.DriveMode_USER, false},
{"Sterring: over right",
fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,50,%d\n", 2998, channel2, channel3, channel4, channel5, channel6, distanceCm),
-1., 1., types.DriveModeUser, false, distanceCm},
-1., 1., events.DriveMode_USER, false},
{"Throttle: over down",
fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 99, channel3, channel4, channel5, channel6, distanceCm),
-1., -1., types.DriveModeUser, false, distanceCm},
-1., -1., events.DriveMode_USER, false},
{"Throttle: down",
fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 998, channel3, channel4, channel5, channel6, distanceCm),
-0.95, -1., types.DriveModeUser, false, distanceCm},
-0.95, -1., events.DriveMode_USER, false},
{"Throttle: stop",
fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 1450, channel3, channel4, channel5, channel6, distanceCm),
-0.03, -1., types.DriveModeUser, false, distanceCm},
-0.03, -1., events.DriveMode_USER, false},
{"Throttle: up",
fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 1948, channel3, channel4, channel5, channel6, distanceCm),
0.99, -1., types.DriveModeUser, false, distanceCm},
0.99, -1., events.DriveMode_USER, false},
{"Throttle: over up",
fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, 2998, channel3, channel4, channel5, channel6, distanceCm),
1., -1., types.DriveModeUser, false, distanceCm},
{"Distance cm",
fmt.Sprintf("12445,%d,%d,%d,%d,%d,%d,50,%d\n", channel1, channel2, channel3, channel4, channel5, channel6, 43),
-1., -1., types.DriveModeUser, false, 43},
{"Distance cm with \r",
fmt.Sprintf("12450,%d,%d,%d,%d,%d,%d,50,%d\r\n", channel1, channel2, channel3, channel4, channel5, channel6, 43),
-1., -1., types.DriveModeUser, false, 43},
1., -1., events.DriveMode_USER, false},
}
for _, c := range cases {
w := bufio.NewWriter(client)
w := bufio.NewWriter(serialClient)
_, err := w.WriteString(c.content)
if err != nil {
t.Errorf("unable to send test content: %v", c.content)
@@ -127,7 +143,7 @@ func TestArduinoPart_Update(t *testing.T) {
t.Error("unable to flush content")
}
time.Sleep(5 * time.Millisecond)
time.Sleep(10 * time.Millisecond)
a.mutex.Lock()
if fmt.Sprintf("%0.2f", a.throttle) != fmt.Sprintf("%0.2f", c.expectedThrottle) {
t.Errorf("%s: bad throttle value, expected: %0.2f, actual: %.2f", c.name, c.expectedThrottle, a.throttle)
@@ -141,15 +157,22 @@ func TestArduinoPart_Update(t *testing.T) {
if a.ctrlRecord != c.expectedSwitchRecord {
t.Errorf("%s: bad switch record, expected: %v, actual:%v", c.name, c.expectedSwitchRecord, a.ctrlRecord)
}
if a.distanceCm != c.expectedDistanceCm {
t.Errorf("%s: bad distanceCm, expected: %v"+
", actual:%v", c.name, c.expectedDistanceCm, a.distanceCm)
}
a.mutex.Unlock()
}
}
func TestPublish(t *testing.T) {
oldPublish := publish
defer func() { publish = oldPublish }()
var muPublishedEvents sync.Mutex
pulishedEvents := make(map[string][]byte)
publish = func(client mqtt.Client, topic string, payload *[]byte) {
muPublishedEvents.Lock()
defer muPublishedEvents.Unlock()
pulishedEvents[topic] = *payload
}
ln, err := net.Listen("tcp", ":8080")
if err != nil {
t.Fatalf("unable to init connection for test")
@@ -169,36 +192,37 @@ func TestPublish(t *testing.T) {
defer conn.Close()
pubFrequency := 100.
p := testtools.NewFakePublisher()
a := Part{serial: conn, pub: p, pubFrequency: pubFrequency, topicBase: "car/part/arduino/"}
a := Part{client: nil, serial: conn, pubFrequency: pubFrequency, topicBase: "car/part/arduino/", cancel:make(chan interface{})}
go a.Start()
defer a.Stop()
cases := []struct {
throttle, steering float64
driveMode types.DriveMode
switchRecord bool
distanceCm int
expectedThrottle, expectedSteering, expectedDriveMode, expectedSwitchRecord, expectedDistance mqttdevice.MqttValue
throttle, steering float32
driveMode events.DriveMode
switchRecord bool
expectedThrottle events.ThrottleMessage
expectedSteering events.SteeringMessage
expectedDriveMode events.DriveModeMessage
expectedSwitchRecord events.SwitchRecordMessage
}{
{-1, 1, types.DriveModeUser, false, 55,
mqttdevice.NewMqttValue(types.Throttle{Value: -1., Confidence: 1.}),
mqttdevice.NewMqttValue(types.Steering{Value: 1.0, Confidence: 1.}),
mqttdevice.NewMqttValue("user"),
mqttdevice.NewMqttValue("OFF"),
mqttdevice.NewMqttValue("55")},
{0, 0, types.DriveModePilot, true, 43,
mqttdevice.NewMqttValue(types.Throttle{Confidence: 1.}),
mqttdevice.NewMqttValue(types.Steering{Confidence: 1.}),
mqttdevice.NewMqttValue("pilot"),
mqttdevice.NewMqttValue("ON"),
mqttdevice.NewMqttValue("43")},
{0.87, -0.58, types.DriveModePilot, true, 21,
mqttdevice.NewMqttValue(types.Throttle{Value: 0.87, Confidence: 1.}),
mqttdevice.NewMqttValue(types.Steering{Value: -0.58, Confidence: 1.}),
mqttdevice.NewMqttValue("pilot"),
mqttdevice.NewMqttValue("ON"),
mqttdevice.NewMqttValue("21")},
{-1, 1, events.DriveMode_USER, false,
events.ThrottleMessage{Throttle: -1., Confidence: 1.},
events.SteeringMessage{Steering: 1.0, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_USER},
events.SwitchRecordMessage{Enabled: false},
},
{0, 0, events.DriveMode_PILOT, true,
events.ThrottleMessage{Throttle: 0., Confidence: 1.},
events.SteeringMessage{Steering: 0., Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true},
},
{0.87, -0.58, events.DriveMode_PILOT, true,
events.ThrottleMessage{Throttle: 0.87, Confidence: 1.},
events.SteeringMessage{Steering: -0.58, Confidence: 1.},
events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
events.SwitchRecordMessage{Enabled: true},
},
}
for _, c := range cases {
@@ -207,27 +231,48 @@ func TestPublish(t *testing.T) {
a.steering = c.steering
a.driveMode = c.driveMode
a.ctrlRecord = c.switchRecord
a.distanceCm = c.distanceCm
a.mutex.Unlock()
time.Sleep(time.Second / time.Duration(int(pubFrequency)))
time.Sleep(500 * time.Millisecond)
if v := p.PublishedEvent("car/part/arduino/throttle"); string(v) != string(c.expectedThrottle) {
t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", v, c.expectedThrottle)
var throttleMsg events.ThrottleMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle"], &throttleMsg)
muPublishedEvents.Unlock()
if throttleMsg.String() != c.expectedThrottle.String() {
t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", throttleMsg, c.expectedThrottle)
}
if v := p.PublishedEvent("car/part/arduino/steering"); string(v) != string(c.expectedSteering) {
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", v, c.expectedSteering)
var steeringMsg events.SteeringMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg)
muPublishedEvents.Unlock()
if steeringMsg.String() != c.expectedSteering.String() {
t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg, c.expectedSteering)
}
if v := p.PublishedEvent("car/part/arduino/drive_mode"); string(v) != string(c.expectedDriveMode) {
val, _ := v.StringValue()
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", val, string(c.expectedDriveMode))
var driveModeMsg events.DriveModeMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg)
muPublishedEvents.Unlock()
if driveModeMsg.String() != c.expectedDriveMode.String() {
t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg, c.expectedDriveMode)
}
if v := p.PublishedEvent("car/part/arduino/switch_record"); string(v) != string(c.expectedSwitchRecord) {
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", v, c.expectedSwitchRecord)
}
if v := p.PublishedEvent("car/part/arduino/distance_cm"); string(v) != string(c.expectedDistance) {
t.Errorf("msg(car/part/arduino/distance_cm): %v, wants %v", v, c.expectedThrottle)
var switchRecordMsg events.SwitchRecordMessage
muPublishedEvents.Lock()
unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg)
muPublishedEvents.Unlock()
if switchRecordMsg.String() != c.expectedSwitchRecord.String() {
t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg, c.expectedSwitchRecord)
}
}
}
func unmarshalMsg(t *testing.T, payload []byte, msg proto.Message) {
err := proto.Unmarshal(payload, msg)
if err != nil {
t.Errorf("unable to unmarshal protobuf content to %T: %v", msg, err)
}
}