def __init__(self, world, foot, historyLength):
self._fsrs = []
self._historyLength = historyLength
self.dataRunningMedian = running_median(self._historyLength)
self.dataRunningMedian.next()
self.lastRead = None
for location in [x+y for x in ['Front', 'Rear'] for y in ['Left', 'Right']]:
var = 'Device/SubDeviceList/%sFoot/FSR/%s/Sensor/Value' % (foot, location)
self._fsrs.append( Sensors.Sensor(world, var) )
def __init__(self, name, world, real_length, world_x=None, world_y=None):
"""
real_length - [cm] real world largest diameter of object.
memory, motion - proxies to ALMemory and ALMotion respectively.
world_x, world_y - [cm] locations in world coordinate frame. World
coordinate frame (reminder) origin is in our team goal center, x
axis is towards opposite goal, y axis is to the left (complete a right
handed coordinate system).
"""
super(Locatable, self).__init__(name)
self._world = world
# cached proxies
self._memory = world._memory
self._motion = world._motion
# longest arc across the object, i.e. a diagonal.
self._real_length = real_length
# This is the world x coordinate. Our x axis is to the right of our goal
# center, and our y is towards the enemy gate^H^H^Hoal. The enemy goal is up.
# (screw Ender)
# default to estimating everything is in center of field.
self.world_x = world_x if world_x is not None else field.MIDFIELD_X
self.world_y = world_y if world_y is not None else field.MIDFIELD_Y # TODO - is't wise?
self.world_heading = 0.0 # default to pointing towards target goal
self.history = RingBuffer(Locatable.HISTORY_SIZE) # stores history for last
# This is the player body frame relative bearing. radians.
self.bearing = 0.0
self.elevation = 0.0
# This is the player body frame relative distance. centimeters.
self.dist = 0.0
self.update_time = 0.0 # time of current update
# previous non zero values
self.last_bearing = 0.0
self.last_dist = 0.0
self.last_elevation = 0.0
self.last_update_time = 0.0 # time of previous update
# Body coordinate system: x is to the right (body is the torso, and generally
# the forward walk direction is along the y axis).
self.body_x = 0.0
self.body_y = 0.0
# upper barrier on speed, used to remove outliers. cm/sec
self.upper_v_limit = 400.0
self.seen = False
self.recently_seen = False # Was the object seen within MISSING_FRAMES_MINIMUM
self.centered = False # whether the distance from the center is smaller then XXX
self.centered_at_pitch_limit = False # centered on X axis, on pitch low limit (looking most upwardly)
self.missingFramesCounter = MISSING_FRAMES_MINIMUM # start as "not seen for inifinity". This is close enough.
# smoothed variables
self.distSmoothed = 0.0
self.distRunningMedian = running_median(3) # TODO: Change to ballEKF/ballLoc?
self.distRunningMedian.next()
# Vision variables defaults
self.centerX = None
self.centerY = None
self.normalized2_centerX = None
self.normalized2_centerY = None
self.x = None
self.y = None
# centered - a copy of some of the values that keeps
# the current searcher values for this target.
self.centered_self = CenteredLocatable(self)
def __init__(self, world, var, historyLength):
super(Sensors.SmoothedSensor, self).__init__(world, var)
self._historyLength = historyLength
self.dataRunningMedian = running_median(self._historyLength)
self.dataRunningMedian.next()
self.lastRead = None
