Compare commits
18 Commits
v0.6.0
...
f1261819f6
Author | SHA1 | Date | |
---|---|---|---|
f1261819f6 | |||
dd7ec4154e | |||
9f26b15c1e | |||
25bea1aab3 | |||
86aef79e66 | |||
efbcac602a | |||
bd4c5d45a4 | |||
e6a5d6f781 | |||
9100cedb38 | |||
a7babb862b | |||
86f91d9f88 | |||
7d27ea866f | |||
a93159df90 | |||
23215bcb4a | |||
9af8f3a770 | |||
2b31a3b7eb | |||
dffa7b4898 | |||
3ae2986580 |
@ -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
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Reference in New Issue
Block a user