fix objects copy

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
 }