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