def GetNextFlipValue(self):
# Get the prior flip value.
flipvalue = self.lpFlip.GetValue()
if (flipvalue is not None):
magFlipvalue = np.abs(flipvalue)
signFlipvalue = np.sign(flipvalue)
else:
magFlipvalue = 0.0
signFlipvalue = 1.0
if signFlipvalue == 0.0:
signFlipvalue = 1.0
angle = self.GetResolvedAngleFiltered()
# Compare distances between angles of (travel - prevorientation) and (travel - flippedprevorientation)
dist_current = np.abs(
CircleFunctions.DistanceCircle(angle, self.angleOfTravelRecent))
dist_flipped = np.abs(
CircleFunctions.DistanceCircle(angle + np.pi,
self.angleOfTravelRecent))
if (self.speed > self.params['tracking']['speedThresholdForTravel']):
if (self.contourinfo.ecc > 0.6):
magFlipvalueNew = 1.0
# Choose the better orientation.
if (dist_flipped < dist_current):
signFlipvalue = -signFlipvalue # Flipped
else:
magFlipvalueNew = magFlipvalue
else:
magFlipvalueNew = magFlipvalue
flipvalueNew = signFlipvalue * magFlipvalueNew
#if 'Fly1' in self.name:
# rospy.logwarn('flipvalue Old,New: % 3.2f, % 3.2f' % (flipvalue, flipvalueNew))
# rospy.logwarn('angle,travel: % 3.2f, % 3.2f dist,flipped: % 3.2f, % 3.2f' % (angle, self.angleOfTravelRecent, dist_current, dist_flipped))
return flipvalueNew
