示例#1
0
文件: sonar.py 项目: alon/burst
 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
示例#2
0
文件: objects.py 项目: alon/burst
    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)
示例#3
0
文件: objects.py 项目: alon/burst
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))
示例#4
0
文件: sonar.py 项目: alon/burst
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)