Corrector implementation

.gitignore

@@ -284,5 +284,3 @@ local.properties
 
 ./build/
 
-pkg/steering/test_result/
-

build-docker.sh

@@ -1,134 +1,50 @@
 #! /bin/bash
 
 IMAGE_NAME=robocar-steering
-BINARY_NAME=rc-steering
 TAG=$(git describe)
 FULL_IMAGE_NAME=docker.io/cyrilix/${IMAGE_NAME}:${TAG}
-OPENCV_VERSION=4.6.0
+BINARY=rc-steering
-SRC_CMD=./cmd/$BINARY_NAME
+
-GOLANG_VERSION=1.19
+GOTAGS="-tags netgo"
 
 image_build(){
-  local containerName=builder
+  local platform=$1
-
-  GOPATH=/go
-
-  buildah from --name ${containerName} docker.io/cyrilix/opencv-buildstage:${OPENCV_VERSION}
-  buildah config --label maintainer="Cyrille Nofficial" "${containerName}"
-
-  buildah copy --from=docker.io/library/golang:${GOLANG_VERSION} "${containerName}" /usr/local/go /usr/local/go
-  buildah config --env GOPATH=/go \
-                 --env PATH=/usr/local/go/bin:$GOPATH/bin:/usr/local/go/bin:/usr/bin:/bin \
-                 "${containerName}"
-
-  buildah run \
-    --env GOPATH=${GOPATH} \
-    "${containerName}" \
-    mkdir -p /src "$GOPATH/src" "$GOPATH/bin"
-
-  buildah run \
-    --env GOPATH=${GOPATH} \
-    "${containerName}" \
-    chmod -R 777 "$GOPATH"
 
 
-  buildah config --env PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:/usr/local/lib64/pkgconfig "${containerName}"
+  GOOS=$(echo $platform | cut -f1 -d/) && \
-  buildah config --workingdir /src/ "${containerName}"
+  GOARCH=$(echo $platform | cut -f2 -d/) && \
+  GOARM=$(echo $platform | cut -f3 -d/ | sed "s/v//" )
+  VARIANT="--variant $(echo $platform | cut -f3 -d/  )"
+  if [[ -z "$GOARM" ]] ;
+  then
+  VARIANT=""
+  fi
 
-  buildah add "${containerName}" . .
+  local binary_suffix="$GOARCH$(echo $platform | cut -f3 -d/ )"
 
-  for platform in "linux/amd64" "linux/arm64" "linux/arm/v7"
+  local containerName="$IMAGE_NAME-$GOARCH$GOARM"
-  do
 
-    GOOS=$(echo "$platform" | cut -f1 -d/) && \
-    GOARCH=$(echo "$platform" | cut -f2 -d/) && \
-    GOARM=$(echo "$platform" | cut -f3 -d/ | sed "s/v//" )
 
-    case $GOARCH in
+  printf "\n\nBuild go binary %s\n\n" "${BINARY}.${binary_suffix}"
-      "amd64")
+  mkdir -p build
-        ARCH=amd64
+  CGO_ENABLED=0 GOOS=${GOOS} GOARCH=${GOARCH} GOARM=${GOARM} go build -mod vendor -a ${GOTAGS} -o "build/${BINARY}.${binary_suffix}" ./cmd/${BINARY}/
-        ARCH_LIB_DIR=/usr/lib/x86_64-linux-gnu
-        EXTRA_LIBS=""
-        CC=gcc
-        CXX=g++
-      ;;
-      "arm64")
-        ARCH=arm64
-        ARCH_LIB_DIR=/usr/lib/aarch64-linux-gnu
-        EXTRA_LIBS="-ltbb"
-        CC=aarch64-linux-gnu-gcc
-        CXX=aarch64-linux-gnu-g++
-      ;;
-      "arm")
-        ARCH=armhf
-        ARCH_LIB_DIR=/usr/lib/arm-linux-gnueabihf
-        EXTRA_LIBS="-ltbb"
-        CC=arm-linux-gnueabihf-gcc
-        CXX=arm-linux-gnueabihf-g++
-      ;;
-    esac
 
-    printf "Build binary for %s\n\n" "${platform}"
+  buildah --os "$GOOS" --arch "$GOARCH" $VARIANT  --name "$containerName" from gcr.io/distroless/static
+  buildah config --user 1234 "$containerName"
+  buildah copy "$containerName" "build/${BINARY}.${binary_suffix}" /go/bin/$BINARY
+  buildah config --entrypoint '["/go/bin/'$BINARY'"]' "${containerName}"
 
-    buildah run \
+  buildah commit --rm --manifest $IMAGE_NAME "${containerName}" "${containerName}"
-      --env CGO_ENABLED=1 \
-      --env CC=${CC} \
-      --env CXX=${CXX} \
-      --env GOOS=${GOOS} \
-      --env GOARCH=${GOARCH} \
-      --env GOARM=${GOARM} \
-      --env CGO_CPPFLAGS="-I/opt/opencv/${ARCH}/include/opencv4/" \
-      --env CGO_LDFLAGS="-L/opt/opencv/${ARCH}/lib -L${ARCH_LIB_DIR} ${EXTRA_LIBS} -lopencv_core -lopencv_face -lopencv_videoio -lopencv_imgproc -lopencv_highgui -lopencv_imgcodecs -lopencv_objdetect -lopencv_features2d -lopencv_video -lopencv_dnn -lopencv_xfeatures2d -lopencv_calib3d -lopencv_photo -lopencv_flann" \
-      --env CGO_CXXFLAGS="--std=c++1z" \
-      "${containerName}" \
-      go build  -tags customenv -a -o ${BINARY_NAME}.${ARCH} ${SRC_CMD}
-
-  done
-  buildah commit --rm ${containerName} ${IMAGE_NAME}-builder
-}
-
-image_final(){
-  local containerName=runtime
-
-  for platform in "linux/amd64" "linux/arm64" "linux/arm/v7"
-  do
-
-    GOOS=$(echo $platform | cut -f1 -d/) && \
-    GOARCH=$(echo $platform | cut -f2 -d/) && \
-    GOARM=$(echo $platform | cut -f3 -d/ | sed "s/v//" )
-    VARIANT="--variant $(echo $platform | cut -f3 -d/  )"
-
-    if [[ -z "$GOARM" ]] ;
-    then
-      VARIANT=""
-    fi
-
-    if [[ "${GOARCH}" == "arm" ]]
-    then
-      BINARY="${BINARY_NAME}.armhf"
-    else
-      BINARY="${BINARY_NAME}.${GOARCH}"
-    fi
-
-    buildah from --name "${containerName}" --os "${GOOS}" --arch "${GOARCH}" ${VARIANT} docker.io/cyrilix/opencv-runtime:${OPENCV_VERSION}
-
-    buildah copy --from ${IMAGE_NAME}-builder  "$containerName" "/src/${BINARY}" /usr/local/bin/${BINARY_NAME}
-
-    buildah config --label maintainer="Cyrille Nofficial" "${containerName}"
-    buildah config --user 1234 "$containerName"
-    buildah config --cmd '' "$containerName"
-    buildah config --entrypoint '[ "/usr/local/bin/'${BINARY_NAME}'" ]' "$containerName"
-
-    buildah commit --rm --manifest ${IMAGE_NAME} ${containerName}
-  done
 }
 
 buildah rmi localhost/$IMAGE_NAME
 buildah manifest rm localhost/${IMAGE_NAME}
 
-image_build
+image_build linux/amd64
+image_build linux/arm64
+image_build linux/arm/v7
+
 
 # push image
-image_final
 printf "\n\nPush manifest to %s\n\n" ${FULL_IMAGE_NAME}
 buildah manifest push --rm -f v2s2 "localhost/$IMAGE_NAME" "docker://$FULL_IMAGE_NAME" --all

cmd/rc-steering/rc-steering.go

@@ -16,10 +16,6 @@ const (
 func main() {
 	var mqttBroker, username, password, clientId string
 	var steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string
-	var imgWidth, imgHeight int
-	var enableObjectsCorrection, enableObjectsCorrectionOnUserMode bool
-	var gridMapConfig, objectsMoveFactorsConfig string
-	var deltaMiddle float64
 
 	mqttQos := cli.InitIntFlag("MQTT_QOS", 0)
 	_, mqttRetain := os.LookupEnv("MQTT_RETAIN")
@@ -31,13 +27,7 @@ func main() {
 	flag.StringVar(&tfSteeringTopic, "mqtt-topic-tf-steering", os.Getenv("MQTT_TOPIC_TF_STEERING"), "Mqtt topic that contains tenorflow steering value, use MQTT_TOPIC_TF_STEERING if args not set")
 	flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic that contains DriveMode value, use MQTT_TOPIC_DRIVE_MODE if args not set")
 	flag.StringVar(&objectsTopic, "mqtt-topic-objects", os.Getenv("MQTT_TOPIC_OBJECTS"), "Mqtt topic that contains Objects from object detection value, use MQTT_TOPIC_OBJECTS if args not set")
-	flag.IntVar(&imgWidth, "image-width", 160, "Video pixels width")
+
-	flag.IntVar(&imgHeight, "image-height", 128, "Video pixels height")
-	flag.BoolVar(&enableObjectsCorrection, "enable-objects-correction", false, "Adjust steering to avoid objects")
-	flag.BoolVar(&enableObjectsCorrectionOnUserMode, "enable-objects-correction-user", false, "Adjust steering to avoid objects on user mode driving")
-	flag.StringVar(&gridMapConfig, "grid-map-config", "", "Json file path to configure grid object correction")
-	flag.StringVar(&objectsMoveFactorsConfig, "objects-move-factors-config", "", "Json file path to configure objects move corrections")
-	flag.Float64Var(&deltaMiddle, "delta-middle", 0.1, "Half Percent zone to interpret as straight")
 	logLevel := zap.LevelFlag("log", zap.InfoLevel, "log level")
 
 	flag.Parse()
@@ -60,36 +50,13 @@ func main() {
 	}()
 	zap.ReplaceGlobals(lgr)
 
-	zap.S().Infof("steering topic                  : %s", steeringTopic)
-	zap.S().Infof("rc topic                        : %s", rcSteeringTopic)
-	zap.S().Infof("tflite steering topic           : %s", tfSteeringTopic)
-	zap.S().Infof("drive mode topic                : %s", driveModeTopic)
-	zap.S().Infof("objects topic                   : %s", objectsTopic)
-	zap.S().Infof("objects correction enabled      : %v", enableObjectsCorrection)
-	zap.S().Infof("objects correction on user mode : %v", enableObjectsCorrectionOnUserMode)
-	zap.S().Infof("grid map file config            : %v", gridMapConfig)
-	zap.S().Infof("objects move factors grid config: %v", objectsMoveFactorsConfig)
-	zap.S().Infof("image width x height            : %v x %v", imgWidth, imgHeight)
-
 	client, err := cli.Connect(mqttBroker, username, password, clientId)
 	if err != nil {
 		log.Fatalf("unable to connect to mqtt bus: %v", err)
 	}
 	defer client.Disconnect(50)
 
-	p := steering.NewController(
+	p := steering.NewController(client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic)
-		client,
-		steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic,
-		steering.WithCorrector(
-			steering.NewGridCorrector(
-				steering.WidthDeltaMiddle(deltaMiddle),
-				steering.WithGridMap(gridMapConfig),
-				steering.WithObjectMoveFactors(objectsMoveFactorsConfig),
-				steering.WithImageSize(imgWidth, imgHeight),
-			),
-		),
-		steering.WithObjectsCorrectionEnabled(enableObjectsCorrection, enableObjectsCorrectionOnUserMode),
-	)
 	defer p.Stop()
 
 	cli.HandleExit(p)

pkg/steering/controller.go

@@ -1,7 +1,6 @@
 package steering
 
 import (
-	"fmt"
 	"github.com/cyrilix/robocar-base/service"
 	"github.com/cyrilix/robocar-protobuf/go/events"
 	mqtt "github.com/eclipse/paho.mqtt.golang"
@@ -10,48 +9,8 @@ import (
 	"sync"
 )
 
-var (
+func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string) *Controller {
-	defaultGridMap = GridMap{
+	return &Controller{
-		DistanceSteps: []float64{0., 0.2, 0.4, 0.6, 0.8, 1.},
-		SteeringSteps: []float64{-1., -0.66, -0.33, 0., 0.33, 0.66, 1.},
-		Data: [][]float64{
-			{0., 0., 0., 0., 0., 0.},
-			{0., 0., 0., 0., 0., 0.},
-			{0., 0., 0.25, -0.25, 0., 0.},
-			{0., 0.25, 0.5, -0.5, -0.25, 0.},
-			{0.25, 0.5, 1, -1, -0.5, -0.25},
-		},
-	}
-	defaultObjectFactors = GridMap{
-		DistanceSteps: []float64{0., 0.2, 0.4, 0.6, 0.8, 1.},
-		SteeringSteps: []float64{-1., -0.66, -0.33, 0., 0.33, 0.66, 1.},
-		Data: [][]float64{
-			{0., 0., 0., 0., 0., 0.},
-			{0., 0., 0., 0., 0., 0.},
-			{0., 0., 0., 0., 0., 0.},
-			{0., 0.25, 0, 0, -0.25, 0.},
-			{0.5, 0.25, 0, 0, -0.5, -0.25},
-		},
-	}
-)
-
-type Option func(c *Controller)
-
-func WithCorrector(c Corrector) Option {
-	return func(ctrl *Controller) {
-		ctrl.corrector = c
-	}
-}
-
-func WithObjectsCorrectionEnabled(enabled, enabledOnUserDrive bool) Option {
-	return func(ctrl *Controller) {
-		ctrl.enableCorrection = enabled
-		ctrl.enableCorrectionOnUser = enabledOnUserDrive
-	}
-}
-
-func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic string, options ...Option) *Controller {
-	c := &Controller{
 		client:          client,
 		steeringTopic:   steeringTopic,
 		driveModeTopic:  driveModeTopic,
@@ -59,12 +18,8 @@ func NewController(client mqtt.Client, steeringTopic, driveModeTopic, rcSteering
 		tfSteeringTopic: tfSteeringTopic,
 		objectsTopic:    objectsTopic,
 		driveMode:       events.DriveMode_USER,
-		corrector:       NewGridCorrector(),
 	}
-	for _, o := range options {
+
-		o(c)
-	}
-	return c
 }
 
 type Controller struct {
@@ -80,28 +35,26 @@ type Controller struct {
 	muObjects sync.RWMutex
 	objects   []*events.Object
 
-	corrector              Corrector
+	debug bool
-	enableCorrection       bool
-	enableCorrectionOnUser bool
 }
 
-func (c *Controller) Start() error {
+func (p *Controller) Start() error {
-	if err := registerCallbacks(c); err != nil {
+	if err := registerCallbacks(p); err != nil {
-		zap.S().Errorf("unable to register callbacks: %v", err)
+		zap.S().Errorf("unable to rgeister callbacks: %v", err)
 		return err
 	}
 
-	c.cancel = make(chan interface{})
+	p.cancel = make(chan interface{})
-	<-c.cancel
+	<-p.cancel
 	return nil
 }
 
-func (c *Controller) Stop() {
+func (p *Controller) Stop() {
-	close(c.cancel)
+	close(p.cancel)
-	service.StopService("throttle", c.client, c.driveModeTopic, c.rcSteeringTopic, c.tfSteeringTopic)
+	service.StopService("throttle", p.client, p.driveModeTopic, p.rcSteeringTopic, p.tfSteeringTopic)
 }
 
-func (c *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
+func (p *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
 	var msg events.ObjectsMessage
 	err := proto.Unmarshal(message.Payload(), &msg)
 	if err != nil {
@@ -109,13 +62,12 @@ func (c *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
 		return
 	}
 
-	c.muObjects.Lock()
+	p.muObjects.Lock()
-	defer c.muObjects.Unlock()
+	defer p.muObjects.Unlock()
-	c.objects = msg.GetObjects()
+	p.objects = msg.GetObjects()
-	zap.S().Debugf("%v object(s) received", len(c.objects))
 }
 
-func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
+func (p *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
 	var msg events.DriveModeMessage
 	err := proto.Unmarshal(message.Payload(), &msg)
 	if err != nil {
@@ -123,89 +75,46 @@ func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
 		return
 	}
 
-	c.muDriveMode.Lock()
+	p.muDriveMode.Lock()
-	defer c.muDriveMode.Unlock()
+	defer p.muDriveMode.Unlock()
-	c.driveMode = msg.GetDriveMode()
+	p.driveMode = msg.GetDriveMode()
 }
 
-func (c *Controller) onRCSteering(_ mqtt.Client, message mqtt.Message) {
+func (p *Controller) onRCSteering(_ mqtt.Client, message mqtt.Message) {
-	c.muDriveMode.RLock()
+	p.muDriveMode.RLock()
-	defer c.muDriveMode.RUnlock()
+	defer p.muDriveMode.RUnlock()
-
+	if p.debug {
-	if c.driveMode != events.DriveMode_USER {
+		var evt events.SteeringMessage
-		return
+		err := proto.Unmarshal(message.Payload(), &evt)
-	}
-
-	payload := message.Payload()
-	evt := &events.SteeringMessage{}
-	err := proto.Unmarshal(payload, evt)
-	if err != nil {
-		zap.S().Debugf("unable to unmarshal rc event: %v", err)
-	} else {
-		zap.S().Debugf("receive steering message from radio command: %0.00f", evt.GetSteering())
-	}
-
-	if c.enableCorrection && c.enableCorrectionOnUser {
-		payload, err = c.adjustSteering(evt)
 		if err != nil {
-			zap.S().Errorf("unable to adjust steering, skip message: %v", err)
+			zap.S().Debugf("unable to unmarshal rc event: %v", err)
-			return
+		} else {
+			zap.S().Debugf("receive steering message from radio command: %0.00f", evt.GetSteering())
 		}
 	}
-	publish(c.client, c.steeringTopic, &payload)
+	if p.driveMode == events.DriveMode_USER {
+		// Republish same content
+		payload := message.Payload()
+		publish(p.client, p.steeringTopic, &payload)
+	}
 }
-
+func (p *Controller) onTFSteering(_ mqtt.Client, message mqtt.Message) {
-func (c *Controller) onTFSteering(_ mqtt.Client, message mqtt.Message) {
+	p.muDriveMode.RLock()
-	c.muDriveMode.RLock()
+	defer p.muDriveMode.RUnlock()
-	defer c.muDriveMode.RUnlock()
+	if p.debug {
-	if c.driveMode != events.DriveMode_PILOT {
+		var evt events.SteeringMessage
-		// User mode, skip new message
+		err := proto.Unmarshal(message.Payload(), &evt)
-		return
-	}
-
-	evt := &events.SteeringMessage{}
-	err := proto.Unmarshal(message.Payload(), evt)
-	if err != nil {
-		zap.S().Errorf("unable to unmarshal tensorflow event: %v", err)
-		return
-	} else {
-		zap.S().Debugf("receive steering message from tensorflow: %0.00f", evt.GetSteering())
-	}
-
-	payload := message.Payload()
-	if c.enableCorrection {
-		payload, err = c.adjustSteering(evt)
 		if err != nil {
-			zap.S().Errorf("unable to adjust steering, skip message: %v", err)
+			zap.S().Debugf("unable to unmarshal tensorflow event: %v", err)
-			return
+		} else {
+			zap.S().Debugf("receive steering message from tensorflow: %0.00f", evt.GetSteering())
 		}
 	}
-
+	if p.driveMode == events.DriveMode_PILOT {
-	publish(c.client, c.steeringTopic, &payload)
+		// Republish same content
-}
+		payload := message.Payload()
-
+		publish(p.client, p.steeringTopic, &payload)
-func (c *Controller) adjustSteering(evt *events.SteeringMessage) ([]byte, error) {
-	steering := float64(evt.GetSteering())
-	steering = c.corrector.AdjustFromObjectPosition(steering, c.Objects())
-	zap.S().Debugf("adjust steering to avoid objects: %v -> %v", evt.GetSteering(), steering)
-	evt.Steering = float32(steering)
-	// override payload content
-	payload, err := proto.Marshal(evt)
-	if err != nil {
-		return nil, fmt.Errorf("unable to marshal steering message with new value, skip message: %v", err)
 	}
-	return payload, nil
-}
-
-func (c *Controller) Objects() []*events.Object {
-	c.muObjects.RLock()
-	defer c.muObjects.RUnlock()
-	res := make([]*events.Object, 0, len(c.objects))
-	for _, o := range c.objects {
-		res = append(res, o)
-	}
-	zap.S().Debugf("copy object from %v to %v", c.objects, res)
-	return res
 }
 
 var registerCallbacks = func(p *Controller) error {

pkg/steering/controller_test.go

@@ -123,198 +123,3 @@ func TestDefaultSteering(t *testing.T) {
 		}
 	}
 }
-
-type StaticCorrector struct {
-	delta float64
-}
-
-func (s *StaticCorrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
-	return s.delta
-}
-
-func TestController_Start(t *testing.T) {
-	oldRegister := registerCallbacks
-	oldPublish := publish
-	defer func() {
-		registerCallbacks = oldRegister
-		publish = oldPublish
-	}()
-	registerCallbacks = func(p *Controller) error {
-		return nil
-	}
-
-	waitPublish := sync.WaitGroup{}
-	var muEventsPublished sync.Mutex
-	eventsPublished := make(map[string][]byte)
-	publish = func(client mqtt.Client, topic string, payload *[]byte) {
-		muEventsPublished.Lock()
-		defer muEventsPublished.Unlock()
-		eventsPublished[topic] = *payload
-		waitPublish.Done()
-	}
-
-	steeringTopic := "topic/steering"
-	driveModeTopic := "topic/driveMode"
-	rcSteeringTopic := "topic/rcSteering"
-	tfSteeringTopic := "topic/tfSteering"
-	objectsTopic := "topic/objects"
-
-	type fields struct {
-		driveMode              events.DriveMode
-		enableCorrection       bool
-		enableCorrectionOnUser bool
-	}
-	type msgEvents struct {
-		driveMode        events.DriveModeMessage
-		rcSteering       events.SteeringMessage
-		tfSteering       events.SteeringMessage
-		expectedSteering events.SteeringMessage
-		objects          events.ObjectsMessage
-	}
-
-	tests := []struct {
-		name               string
-		fields             fields
-		msgEvents          msgEvents
-		correctionOnObject float64
-		want               events.SteeringMessage
-		wantErr            bool
-	}{
-		{
-			name: "On user drive mode, none correction",
-			fields: fields{
-				driveMode:              events.DriveMode_USER,
-				enableCorrection:       false,
-				enableCorrectionOnUser: false,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-		},
-		{
-			name: "On pilot drive mode, none correction",
-			fields: fields{
-				driveMode:              events.DriveMode_PILOT,
-				enableCorrection:       false,
-				enableCorrectionOnUser: false,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-		},
-		{
-			name: "On pilot drive mode, correction enabled",
-			fields: fields{
-				driveMode:              events.DriveMode_PILOT,
-				enableCorrection:       true,
-				enableCorrectionOnUser: false,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
-		},
-		{
-			name: "On pilot drive mode, all corrections enabled",
-			fields: fields{
-				driveMode:              events.DriveMode_PILOT,
-				enableCorrection:       true,
-				enableCorrectionOnUser: true,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_PILOT},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
-		},
-		{
-			name: "On user drive mode, only correction PILOT enabled",
-			fields: fields{
-				driveMode:              events.DriveMode_PILOT,
-				enableCorrection:       true,
-				enableCorrectionOnUser: false,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-		},
-		{
-			name: "On user drive mode, all corrections enabled",
-			fields: fields{
-				driveMode:              events.DriveMode_USER,
-				enableCorrection:       true,
-				enableCorrectionOnUser: true,
-			},
-			msgEvents: msgEvents{
-				driveMode:  events.DriveModeMessage{DriveMode: events.DriveMode_USER},
-				rcSteering: events.SteeringMessage{Steering: 0.3, Confidence: 1.0},
-				tfSteering: events.SteeringMessage{Steering: 0.4, Confidence: 1.0},
-				objects:    events.ObjectsMessage{Objects: []*events.Object{&objectOnMiddleNear}},
-			},
-			correctionOnObject: 0.5,
-			// Get rc value without correction
-			want: events.SteeringMessage{Steering: 0.5, Confidence: 1.0},
-		},
-	}
-
-	for _, tt := range tests {
-		t.Run(tt.name, func(t *testing.T) {
-			c := NewController(nil,
-				steeringTopic, driveModeTopic, rcSteeringTopic, tfSteeringTopic, objectsTopic,
-				WithObjectsCorrectionEnabled(tt.fields.enableCorrection, tt.fields.enableCorrectionOnUser),
-				WithCorrector(&StaticCorrector{delta: tt.correctionOnObject}),
-			)
-			go c.Start()
-			time.Sleep(1 * time.Millisecond)
-
-			// Publish events and wait generation of new steering message
-			waitPublish.Add(1)
-			c.onDriveMode(nil, testtools.NewFakeMessageFromProtobuf(driveModeTopic, &tt.msgEvents.driveMode))
-			c.onRCSteering(nil, testtools.NewFakeMessageFromProtobuf(rcSteeringTopic, &tt.msgEvents.rcSteering))
-			c.onTFSteering(nil, testtools.NewFakeMessageFromProtobuf(tfSteeringTopic, &tt.msgEvents.tfSteering))
-			c.onObjects(nil, testtools.NewFakeMessageFromProtobuf(objectsTopic, &tt.msgEvents.objects))
-			waitPublish.Wait()
-
-			var msg events.SteeringMessage
-			muEventsPublished.Lock()
-			err := proto.Unmarshal(eventsPublished[steeringTopic], &msg)
-			if err != nil {
-				t.Errorf("unable to unmarshall response: %v", err)
-				t.Fail()
-			}
-			muEventsPublished.Unlock()
-
-			if msg.GetSteering() != tt.want.GetSteering() {
-				t.Errorf("bad msg value for mode %v: %v, wants %v", c.driveMode.String(), msg.GetSteering(), tt.want.GetSteering())
-			}
-
-		})
-	}
-}

pkg/steering/corrector.go

@@ -8,86 +8,17 @@ import (
 	"os"
 )
 
-type Corrector interface {
+func NewCorrector(gridMap *GridMap, objectMoveFactors *GridMap) *Corrector {
-	AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64
+	return &Corrector{
-}
+		gridMap:           gridMap,
-type OptionCorrector func(c *GridCorrector)
+		objectMoveFactors: objectMoveFactors,
-
-func WithGridMap(configPath string) OptionCorrector {
-	var gm *GridMap
-	if configPath == "" {
-		zap.S().Warnf("no configuration defined for grid map, use default")
-		gm = &defaultGridMap
-	} else {
-		var err error
-		gm, err = loadConfig(configPath)
-		if err != nil {
-			zap.S().Panicf("unable to load grid-map config from file '%v': %w", configPath, err)
-		}
-	}
-	return func(c *GridCorrector) {
-		c.gridMap = gm
-	}
-}
-
-func WithObjectMoveFactors(configPath string) OptionCorrector {
-	var omf *GridMap
-	if configPath == "" {
-		zap.S().Warnf("no configuration defined for objects move factors, use default")
-		omf = &defaultObjectFactors
-	} else {
-		var err error
-		omf, err = loadConfig(configPath)
-		if err != nil {
-			zap.S().Panicf("unable to load objects move factors config from file '%v': %w", configPath, err)
-		}
-	}
-	return func(c *GridCorrector) {
-		c.objectMoveFactors = omf
-	}
-}
-
-func loadConfig(configPath string) (*GridMap, error) {
-	content, err := os.ReadFile(configPath)
-	if err != nil {
-		return nil, fmt.Errorf("unable to load grid-map config from file '%v': %w", configPath, err)
-	}
-	var gm GridMap
-	err = json.Unmarshal(content, &gm)
-	if err != nil {
-		return nil, fmt.Errorf("unable to unmarshal json config '%s': %w", configPath, err)
-	}
-	return &gm, nil
-}
-
-func WithImageSize(width, height int) OptionCorrector {
-	return func(c *GridCorrector) {
-		c.imgWidth = width
-		c.imgHeight = height
-	}
-}
-
-func WidthDeltaMiddle(d float64) OptionCorrector {
-	return func(c *GridCorrector) {
-		c.deltaMiddle = d
-	}
-
-}
-func NewGridCorrector(options ...OptionCorrector) *GridCorrector {
-	c := &GridCorrector{
-		gridMap:           &defaultGridMap,
-		objectMoveFactors: &defaultObjectFactors,
 		deltaMiddle:       0.1,
 		imgWidth:          160,
 		imgHeight:         120,
 	}
-	for _, o := range options {
-		o(c)
-	}
-	return c
 }
 
-type GridCorrector struct {
+type Corrector struct {
 	gridMap             *GridMap
 	objectMoveFactors   *GridMap
 	deltaMiddle         float64
@@ -130,8 +61,7 @@ AdjustFromObjectPosition modify steering value according object positions
     40% |-----|-----|-----|-----|-----|-----|
     :   | ... | ... | ... | ... | ... | ... |
 */
-func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
+func (c *Corrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
-	zap.S().Debugf("%v objects to avoid", len(objects))
 	if len(objects) == 0 {
 		return currentSteering
 	}
@@ -140,7 +70,7 @@ func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, object
 	// get nearest object
 	nearest, err := c.nearObject(grpObjs)
 	if err != nil {
-		zap.S().Warnf("unexpected error on nearest search object, ignore objects: %v", err)
+		zap.S().Warnf("unexpected error on nearest seach object, ignore objects: %v", err)
 		return currentSteering
 	}
 
@@ -174,11 +104,10 @@ func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, object
 	}
 }
 
-func (c *GridCorrector) computeDeviation(nearest *events.Object) float64 {
+func (c *Corrector) computeDeviation(nearest *events.Object) float64 {
 	var delta float64
 	var err error
 
-	zap.S().Debugf("search delta value for bottom limit: %v", nearest.Bottom)
 	if nearest.Left < 0 && nearest.Right < 0 {
 		delta, err = c.gridMap.ValueOf(float64(nearest.Right)*2-1., float64(nearest.Bottom))
 	}
@@ -191,11 +120,10 @@ func (c *GridCorrector) computeDeviation(nearest *events.Object) float64 {
 		zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err)
 		delta = 0
 	}
-	zap.S().Debugf("new deviation computed: %v", delta)
 	return delta
 }
 
-func (c *GridCorrector) nearObject(objects []*events.Object) (*events.Object, error) {
+func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error) {
 	if len(objects) == 0 {
 		return nil, fmt.Errorf("list objects must contain at least one object")
 	}

pkg/steering/corrector_test.go

@@ -57,6 +57,31 @@ var (
 	}
 )
 
+var (
+	defaultGridMap = GridMap{
+		DistanceSteps: []float64{0., 0.2, 0.4, 0.6, 0.8, 1.},
+		SteeringSteps: []float64{-1., -0.66, -0.33, 0., 0.33, 0.66, 1.},
+		Data: [][]float64{
+			{0., 0., 0., 0., 0., 0.},
+			{0., 0., 0., 0., 0., 0.},
+			{0., 0., 0.25, -0.25, 0., 0.},
+			{0., 0.25, 0.5, -0.5, -0.25, 0.},
+			{0.25, 0.5, 1, -1, -0.5, -0.25},
+		},
+	}
+	defaultObjectFactors = GridMap{
+		DistanceSteps: []float64{0., 0.2, 0.4, 0.6, 0.8, 1.},
+		SteeringSteps: []float64{-1., -0.66, -0.33, 0., 0.33, 0.66, 1.},
+		Data: [][]float64{
+			{0., 0., 0., 0., 0., 0.},
+			{0., 0., 0., 0., 0., 0.},
+			{0., 0., 0., 0., 0., 0.},
+			{0., 0.25, 0, 0, -0.25, 0.},
+			{0.5, 0.25, 0, 0, -0.5, -0.25},
+		},
+	}
+)
+
 func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 	type args struct {
 		currentSteering float64
@@ -159,7 +184,7 @@ func TestCorrector_AdjustFromObjectPosition(t *testing.T) {
 	}
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {
-			c := NewGridCorrector()
+			c := NewCorrector(&defaultGridMap, &defaultObjectFactors)
 			if got := c.AdjustFromObjectPosition(tt.args.currentSteering, tt.args.objects); got != tt.want {
 				t.Errorf("AdjustFromObjectPosition() = %v, want %v", got, tt.want)
 			}
@@ -204,7 +229,7 @@ func TestCorrector_nearObject(t *testing.T) {
 	}
 	for _, tt := range tests {
 		t.Run(tt.name, func(t *testing.T) {
-			c := &GridCorrector{}
+			c := &Corrector{}
 			got, err := c.nearObject(tt.args.objects)
 			if (err != nil) != tt.wantErr {
 				t.Errorf("nearObject() error = %v, wantErr %v", err, tt.wantErr)
@@ -413,67 +438,3 @@ func TestGridMap_ValueOf(t *testing.T) {
 		})
 	}
 }
-
-func TestWithGridMap(t *testing.T) {
-	type args struct {
-		config string
-	}
-	tests := []struct {
-		name string
-		args args
-		want GridMap
-	}{
-		{
-			name: "default value",
-			args: args{config: ""},
-			want: defaultGridMap,
-		},
-		{
-			name: "load config",
-			args: args{config: "test_data/config.json"},
-			want: defaultGridMap,
-		},
-	}
-	for _, tt := range tests {
-		t.Run(tt.name, func(t *testing.T) {
-			c := GridCorrector{}
-			got := WithGridMap(tt.args.config)
-			got(&c)
-			if !reflect.DeepEqual(*c.gridMap, tt.want) {
-				t.Errorf("WithGridMap() = %v, want %v", *c.gridMap, tt.want)
-			}
-		})
-	}
-}
-
-func TestWithObjectMoveFactors(t *testing.T) {
-	type args struct {
-		config string
-	}
-	tests := []struct {
-		name string
-		args args
-		want GridMap
-	}{
-		{
-			name: "default value",
-			args: args{config: ""},
-			want: defaultObjectFactors,
-		},
-		{
-			name: "load config",
-			args: args{config: "test_data/omf-config.json"},
-			want: defaultObjectFactors,
-		},
-	}
-	for _, tt := range tests {
-		t.Run(tt.name, func(t *testing.T) {
-			c := GridCorrector{}
-			got := WithObjectMoveFactors(tt.args.config)
-			got(&c)
-			if !reflect.DeepEqual(*c.objectMoveFactors, tt.want) {
-				t.Errorf("WithObjectMoveFactors() = %v, want %v", *c.objectMoveFactors, tt.want)
-			}
-		})
-	}
-}

pkg/steering/test_data/omf-config.json

@@ -1,11 +0,0 @@
-{
-  "steering_steps":[-1, -0.66, -0.33, 0, 0.33, 0.66, 1],
-  "distance_steps": [0, 0.2, 0.4, 0.6, 0.8, 1],
-  "data": [
-	[0, 0, 0, 0, 0, 0],
-	[0, 0, 0, 0, 0, 0],
-	[0, 0, 0, 0, 0, 0],
-	[0, 0.25, 0, 0, -0.25, 0],
-	[0.5, 0.25, 0, 0, -0.5, -0.25]
-  ]
-}