18 Commits

Author SHA1 Message Date
f1261819f6 chore: update gitignore 2022-08-30 23:40:20 +02:00
dd7ec4154e build: update docker image with opencv 4.6.0 2022-08-30 23:39:15 +02:00
9f26b15c1e WIP 2022-08-29 11:42:25 +02:00
25bea1aab3 WIP 2022-08-28 23:33:51 +02:00
86aef79e66 WIP 2022-08-28 12:06:43 +02:00
efbcac602a Corrector implementation 2022-08-27 19:03:09 +02:00
bd4c5d45a4 WIP 2022-08-24 13:54:36 +02:00
e6a5d6f781 wip 2022-08-23 22:08:07 +02:00
9100cedb38 build: upgrade to go 1.19 2022-08-23 18:28:15 +02:00
a7babb862b WIP 2022-08-23 13:53:59 +02:00
86f91d9f88 WIP 2022-08-23 13:24:31 +02:00
7d27ea866f WIP 2022-08-22 23:56:13 +02:00
a93159df90 WIP 2022-08-22 19:51:44 +02:00
23215bcb4a wip 2022-08-22 14:06:27 +02:00
9af8f3a770 Consume objects from mqtt topic 2022-08-22 13:12:12 +02:00
2b31a3b7eb Test GrouBBoxes OK 2022-08-22 12:06:41 +02:00
dffa7b4898 wip 2022-08-21 23:01:18 +02:00
3ae2986580 wip 2022-08-21 22:35:02 +02:00
2 changed files with 2 additions and 10 deletions

View File

@ -112,7 +112,6 @@ func (c *Controller) onObjects(_ mqtt.Client, message mqtt.Message) {
c.muObjects.Lock() c.muObjects.Lock()
defer c.muObjects.Unlock() defer c.muObjects.Unlock()
c.objects = msg.GetObjects() c.objects = msg.GetObjects()
zap.S().Debugf("%v object(s) received", len(c.objects))
} }
func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) { func (c *Controller) onDriveMode(_ mqtt.Client, message mqtt.Message) {
@ -187,7 +186,6 @@ func (c *Controller) onTFSteering(_ mqtt.Client, message mqtt.Message) {
func (c *Controller) adjustSteering(evt *events.SteeringMessage) ([]byte, error) { func (c *Controller) adjustSteering(evt *events.SteeringMessage) ([]byte, error) {
steering := float64(evt.GetSteering()) steering := float64(evt.GetSteering())
steering = c.corrector.AdjustFromObjectPosition(steering, c.Objects()) steering = c.corrector.AdjustFromObjectPosition(steering, c.Objects())
zap.S().Debugf("adjust steering to avoid objects: %v -> %v", evt.GetSteering(), steering)
evt.Steering = float32(steering) evt.Steering = float32(steering)
// override payload content // override payload content
payload, err := proto.Marshal(evt) payload, err := proto.Marshal(evt)
@ -201,10 +199,7 @@ func (c *Controller) Objects() []*events.Object {
c.muObjects.RLock() c.muObjects.RLock()
defer c.muObjects.RUnlock() defer c.muObjects.RUnlock()
res := make([]*events.Object, 0, len(c.objects)) res := make([]*events.Object, 0, len(c.objects))
for _, o := range c.objects { copy(res, c.objects)
res = append(res, o)
}
zap.S().Debugf("copy object from %v to %v", c.objects, res)
return res return res
} }

View File

@ -131,7 +131,6 @@ AdjustFromObjectPosition modify steering value according object positions
: | ... | ... | ... | ... | ... | ... | : | ... | ... | ... | ... | ... | ... |
*/ */
func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 { func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
zap.S().Debugf("%v objects to avoid", len(objects))
if len(objects) == 0 { if len(objects) == 0 {
return currentSteering return currentSteering
} }
@ -140,7 +139,7 @@ func (c *GridCorrector) AdjustFromObjectPosition(currentSteering float64, object
// get nearest object // get nearest object
nearest, err := c.nearObject(grpObjs) nearest, err := c.nearObject(grpObjs)
if err != nil { 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 return currentSteering
} }
@ -178,7 +177,6 @@ func (c *GridCorrector) computeDeviation(nearest *events.Object) float64 {
var delta float64 var delta float64
var err error var err error
zap.S().Debugf("search delta value for bottom limit: %v", nearest.Bottom)
if nearest.Left < 0 && nearest.Right < 0 { if nearest.Left < 0 && nearest.Right < 0 {
delta, err = c.gridMap.ValueOf(float64(nearest.Right)*2-1., float64(nearest.Bottom)) delta, err = c.gridMap.ValueOf(float64(nearest.Right)*2-1., float64(nearest.Bottom))
} }
@ -191,7 +189,6 @@ func (c *GridCorrector) computeDeviation(nearest *events.Object) float64 {
zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err) zap.S().Warnf("unable to compute delta to apply to steering, skip correction: %v", err)
delta = 0 delta = 0
} }
zap.S().Debugf("new deviation computed: %v", delta)
return delta return delta
} }