fix objects copy

.gitignore

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

build-docker.sh

@@ -1,50 +1,134 @@
 #! /bin/bash
 
 IMAGE_NAME=robocar-steering
+BINARY_NAME=rc-steering
 TAG=$(git describe)
 FULL_IMAGE_NAME=docker.io/cyrilix/${IMAGE_NAME}:${TAG}
-BINARY=rc-steering
+OPENCV_VERSION=4.6.0
-
+SRC_CMD=./cmd/$BINARY_NAME
-GOTAGS="-tags netgo"
+GOLANG_VERSION=1.19
 
 image_build(){
-  local platform=$1
+  local containerName=builder
+
+  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"
 
 
-  GOOS=$(echo $platform | cut -f1 -d/) && \
+  buildah config --env PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:/usr/local/lib64/pkgconfig "${containerName}"
-  GOARCH=$(echo $platform | cut -f2 -d/) && \
+  buildah config --workingdir /src/ "${containerName}"
-  GOARM=$(echo $platform | cut -f3 -d/ | sed "s/v//" )
-  VARIANT="--variant $(echo $platform | cut -f3 -d/  )"
-  if [[ -z "$GOARM" ]] ;
-  then
-  VARIANT=""
-  fi
 
-  local binary_suffix="$GOARCH$(echo $platform | cut -f3 -d/ )"
+  buildah add "${containerName}" . .
 
-  local containerName="$IMAGE_NAME-$GOARCH$GOARM"
+  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//" )
 
-  printf "\n\nBuild go binary %s\n\n" "${BINARY}.${binary_suffix}"
+    case $GOARCH in
-  mkdir -p build
+      "amd64")
-  CGO_ENABLED=0 GOOS=${GOOS} GOARCH=${GOARCH} GOARM=${GOARM} go build -mod vendor -a ${GOTAGS} -o "build/${BINARY}.${binary_suffix}" ./cmd/${BINARY}/
+        ARCH=amd64
+        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
 
-  buildah --os "$GOOS" --arch "$GOARCH" $VARIANT  --name "$containerName" from gcr.io/distroless/static
+    printf "Build binary for %s\n\n" "${platform}"
-  buildah config --user 1234 "$containerName"
-  buildah copy "$containerName" "build/${BINARY}.${binary_suffix}" /go/bin/$BINARY
-  buildah config --entrypoint '["/go/bin/'$BINARY'"]' "${containerName}"
 
-  buildah commit --rm --manifest $IMAGE_NAME "${containerName}" "${containerName}"
+    buildah run \
+      --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 linux/amd64
+image_build
-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

pkg/steering/controller.go

@@ -112,6 +112,7 @@ func (c *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
 	c.muObjects.Lock()
 	defer c.muObjects.Unlock()
 	c.objects = msg.GetObjects()
+	zap.S().Debugf("%v object(s) received", len(c.objects))
 }
 
 func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@@ -186,6 +187,7 @@ func (c *Controller) onTFSteering(_ mqtt.Client, message mqtt.Message) {
 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)
@@ -199,7 +201,10 @@ func (c *Controller) Objects() []*events.Object {
 	c.muObjects.RLock()
 	defer c.muObjects.RUnlock()
 	res := make([]*events.Object, 0, len(c.objects))
-	copy(res, 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
 }
 

pkg/steering/corrector.go

@@ -131,6 +131,7 @@ AdjustFromObjectPosition modify steering value according object positions
     :   | ... | ... | ... | ... | ... | ... |
 */
 func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
+	zap.S().Debugf("%v objects to avoid", len(objects))
 	if len(objects) == 0 {
 		return currentSteering
 	}
@@ -139,7 +140,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 seach object, ignore objects: %v", err)
+		zap.S().Warnf("unexpected error on nearest search object, ignore objects: %v", err)
 		return currentSteering
 	}
 
@@ -177,6 +178,7 @@ func (c *GridCorrector) 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))
 	}
@@ -189,6 +191,7 @@ 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
 }