def __init__(self, world):
print "__init__"
if burst.options.run_sonar:
SONAR_EXTRACTOR_SUBSCRIBE(
module=world._sonar.post,
myname='',
dt_ms=500)
world.addMemoryVars([US_DISTANCES_VARNAME])
self._world = world
self._sonarHistory = RingBuffer(US_HISTORY_SIZE)
self._obstacleSeen = False
self._lastData = None # last raw data (for deciding when data changed)
self._lastReading = None # last (side / distance)
self._obstacle_lost_frames_counter = 0
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)
class Locatable(Nameable):
""" stupid name. It is short for "something that can be seen, holds a position,
has a limited velocity which can be estimated, and is also interesting to the
soccer game"
Because of the way the vision system we use (northern's) works, we keep things
in polar coordinates - bearing in radians, distance in centimeters.
"""
REPORT_JUMP_ERRORS = False
HISTORY_SIZE = 20
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 get_xy(self):
return self.world_x, self.world_y
xy = property(get_xy)
def centering_error(self, normalized_error_x=DEFAULT_NORMALIZED_CENTERING_X_ERROR,
normalized_error_y=DEFAULT_NORMALIZED_CENTERING_Y_ERROR):
""" calculate normalized error from image center for object, using
centerX and centerY from vision, using given defaults for what "centered"
means (i.e. what the margins are).
Return: centered, x_normalized_error, y_normalized_error
errors are in [-1, 1]
"""
# TODO - using target.centerX and target.centerY without looking at update_time is broken.
# Normalize ball X between 1 (left) to -1 (right)
assert(normalized_error_x > 0 and normalized_error_y > 0)
xNormalized = normalized2_image_width(self.centerX)
# Normalize ball Y between 1 (top) to -1 (bottom)
yNormalized = normalized2_image_height(self.centerY)
cur_pitch = self._world.getAngle('HeadPitch')
pitch_barrier = CENTERING_MINIMUM_PITCH
elevation_on_upper_edge = cur_pitch < pitch_barrier
centered_at_pitch_limit = (elevation_on_upper_edge and yNormalized < 0 and
abs(xNormalized) < normalized_error_x)
centered = (abs(xNormalized) <= normalized_error_x and
abs(yNormalized) <= normalized_error_y)
return centered, centered_at_pitch_limit ,xNormalized, yNormalized
def calc_recently_seen(self, new_seen):
""" sometimes we don't want to know if the object is visible this frame,
it is enough if it is visible for the last few frames
"""
new_recently_seen = new_seen
if new_seen: # otherwise new_elevation is 'None'
self.missingFramesCounter = 0
else:
# only update new_seen with a False value when some minimal "missing" frame counter is reach
self.missingFramesCounter += 1
if self.missingFramesCounter < MISSING_FRAMES_MINIMUM:
new_recently_seen = True
return new_recently_seen
# History related stuff
HISTORY_LABELS = ['time', 'distance', 'bearing', 'head_yaw', 'head_pitch']
HIST_TIME, HIST_DIST, HIST_BEARING, HIST_YAW, HIST_PITCH = range(5)
def _record_current_state(self):
""" pushes new values into the history buffer. called from
update_location_body_coordinates
"""
self.history.ring_append([self.update_time, self.dist, self.bearing,
self.centered_self.head_yaw, self.centered_self.head_pitch])
@property
def seenWithinOneFrame(self):
min_dt = 0.2 # TODO - constant this. Also - eventmanager dependency?
return self.seen or (
self._world.time - self.history[-1][self.HIST_TIME] <= min_dt if self.history[-1] else False)
@once_per_step
def update_location_body_coordinates(self, new_dist, new_bearing, new_elevation):
""" We only update the values if the move looks plausible.
TODO: This is a first order computation of velocity and position,
removing outright outliers only. To be upgraded to a real localization
system (i.e. reuse northern's code as a module, export variables).
"""
update_time = self._world.time
dt = update_time - self.update_time
if dt <= 0.0:
print "TIME ERROR: dt = %s, not updating anything" % dt
import pdb; pdb.set_trace()
return
body_x, body_y = new_dist * cos(new_bearing), new_dist * sin(new_bearing)
dx, dy = body_x - self.body_x, body_y - self.body_y
if dx**2 + dy**2 > (dt * self.upper_v_limit)**2:
# no way this body jumped that quickly
if self.REPORT_JUMP_ERRORS:
print "JUMP ERROR: %s:%s - %s, %s -> %s, %s - bad new position value, not updating" % (
self.__class__.__name__, self.name, self.body_x, self.body_y, body_x, body_y)
return
self.last_update_time, self.last_dist, self.last_elevation, self.last_bearing = (
self.update_time, self.dist, self.elevation, self.bearing)
self.bearing = new_bearing
self.dist = new_dist
self._record_current_state()
self.distSmoothed = self.distRunningMedian.send(new_dist)
#if isinstance(self, Ball):
# print "%s: self.dist, self.distSmoothed: %3.3f %3.3f" % (self, self.dist, self.distSmoothed)
self.elevation = new_elevation
self.update_time = update_time
self.body_x, self.body_y = body_x, body_y
self.vx, self.vy = dx/dt, dy/dt
def update_centered(self):
""" call this after all vision variables have been updated. It computes
the centering error (distance from center along both axis normalized to
[-1, 1]) and stores in self.centered_self the most centered sighting with
all parameters including body angles. """
(self.centered,
self.centered_at_pitch_limit,
self.normalized2_centerX,
self.normalized2_centerY,
) = self.centering_error()
if burst.options.debug:
print "%s: update_centered: %s %1.2f %1.2f" % (self.name,
self.centered, self.normalized2_centerX, self.normalized2_centerY)
self.centered_self._update() # must be after updating self.normalized2_center{X,Y}
def updateLocatable(self, new_seen):
""" helpers for inheriting classes from Locatable, specifically used by GoalPost, also
for Goal which updates the GoalPosts """
self.seen = new_seen
self.recently_seen = self.calc_recently_seen(new_seen)
if self.seen:
self.update_centered()
self.update_time = self._world.time
def __str__(self):
return "<%s; (%3.2f,%3.2f,%3.2f,%3.2f), cl=%s>" % (self.name, self.update_time, self.dist, self.bearing, self.elevation, str(self.centered_self))
class Sonar(object):
def __init__(self, world):
print "__init__"
if burst.options.run_sonar:
SONAR_EXTRACTOR_SUBSCRIBE(
module=world._sonar.post,
myname='',
dt_ms=500)
world.addMemoryVars([US_DISTANCES_VARNAME])
self._world = world
self._sonarHistory = RingBuffer(US_HISTORY_SIZE)
self._obstacleSeen = False
self._lastData = None # last raw data (for deciding when data changed)
self._lastReading = None # last (side / distance)
self._obstacle_lost_frames_counter = 0
# valid only when obstacle seen/in_frame events occur, otherwise, returns None
def getLastReading(self):
return self._lastReading
def readSonarDistances(self, data):
# print "Sonar: LEFT Got %f" % (data[LEFT])
# print "Sonar: RIGHT Got %f" % (data[RIGHT])
self._sonarHistory.ring_append([data[LEFT], data[RIGHT]])
def calc_events(self, events, deferreds):
if not burst.options.run_sonar:
return
new_data = self._world.vars[US_DISTANCES_VARNAME]
# make sure sonar data is valid
if self._lastData != new_data and new_data and (len(new_data) >= 2):
self._lastData = new_data
self.readSonarDistances(new_data)
newEvent = None
# if at least one reading shows an obstacle, do more complex analysis
if min(new_data[LEFT], new_data[RIGHT]) < US_NEAR_DISTANCE:
self._lastReading = self.calcObstacleFromSonar(self._sonarHistory)
if self._obstacleSeen:
newEvent = EVENT_OBSTACLE_IN_FRAME
#print "Sonar: IN_FRAME obstacle (on %s, distance of %f)" % (self._lastReading)
else:
# new obstacle found, report it
newEvent = EVENT_OBSTACLE_SEEN
#print "Sonar: SEEN obstacle (on %s, distance of %f)" % (self._lastReading)
self._obstacleSeen = True
self._obstacle_lost_frames_counter = 0
else:
if self._obstacleSeen:
self._obstacle_lost_frames_counter += 1
if self._obstacle_lost_frames_counter >= US_FRAMES_TILL_LOST:
newEvent = EVENT_OBSTACLE_LOST
self._obstacleSeen = False
self._obstacle_lost_frames_counter = 0
self._lastReading = None
#print "Sonar: LOST obstacle (last seen on %s, distance of %f)" % (self._lastReading)
if (newEvent != None):
events.add(newEvent)
else:
#print "Sonar: either no reading or same as last reading - ignoring..."
self._lastReading = None
##
# Authors: Sonia Anshtein Shafran & Rony Fragin.
# Input: Sonar history
# Each sample is an array of 2 - left and right readings.
# Output: Array of 2 - evaluation of both direction of object and its distance.
def calcObstacleFromSonar(self, history):
# Initialize variables.
vote_thresh_left= 0
vote_thresh_right= 0
vote_majority_left= 0
vote_majority_right= 0
diff= 9999
min_left= 9999
min_right= 9999
upper_thresh= US_NEAR_DISTANCE # TODO: get this number from personalization file (minimal noise left/right when no obstacle). Was 1.0.
lower_thresh= 0.045
# Run on data and get the needed parameters for evaluation.
for point in history:
if point == None:
break
left_val = point[LEFT]
right_val = point[RIGHT]
currDiff= abs(left_val-right_val)
# New minimal difference between left and right readings.
if currDiff<diff:
# Readings of an existing object [ below the threshold ].
if left_val<=upper_thresh or right_val<=upper_thresh:
# Some weird bug when left and right readings are the same.
if currDiff>0:
diff= currDiff;
# New left minimum reading.
if left_val<min_left:
min_left= left_val
# New right minimum reading.
if right_val<min_right:
min_right= right_val
# Left reading was below upper threshold - valid.
if left_val<upper_thresh:
# Threshold voting counts the number of times we
# got a valid reading from the sensor.
vote_thresh_left= vote_thresh_left + 1
# Right reading was below upper threshold - valid.
if right_val<upper_thresh:
vote_thresh_right= vote_thresh_right + 1
# Left value was lower than the right value.
if left_val<=right_val:
# Majority voting counts the number of times a
# reading of one sensor was lower than the other's.
vote_majority_left= vote_majority_left + 1
# Right value was lower than the left value.
if right_val<=left_val:
vote_majority_right= vote_majority_right + 1
# Find min of min.
if min_left<min_right:
min_min= min_left
else:
min_min= min_right
# If min of min is above upper threshold,
# the reading is invalid.
if min_min>upper_thresh:
return ("nothing", 0)
else:
# If minimal difference is below lower threshold,
# the direction is center.
if diff<lower_thresh:
return ("center", min_min)
else:
# If more threshold votes were cast to the left,
# meaning left direction is dominent.
if vote_thresh_left>vote_thresh_right:
return ("left", min_min)
# If more threshold votes were cast to the right,
# meaning right direction is dominent.
elif vote_thresh_left<vote_thresh_right:
return ("right", min_min)
# Both sides got the same amount of threshold votes.
else:
# If more majority votes were cast to the left,
# meaning left direction is dominent.
if vote_majority_left>vote_majority_right:
return ("left", min_min)
# If more majority votes were cast to the right,
# meaning right direction is dominent.
elif vote_majority_left<vote_majority_right:
return ("right", min_min)
# No majority to any side, center is dominent.
else:
return ("center", min_min)
