예제 #1
0
 def load_mapfile(self, filename):
     self.map = CourseMap(filename)
예제 #2
0
 def load_mapfile(self, filename):
   self.map = CourseMap(filename)
예제 #3
0
class MapNode(object):
    def __init__(self, screen):
        self.screen = screen
        self.screen.nodelay(1)  # make screen.getch() non-blocking
        self.curr_segment = None
        self.prev_segment = None
        self.steer = 0
        self.odometer = 0.0
        self.lap_start = 0.0
        self.compass = 0

    def load_mapfile(self, filename):
        self.map = CourseMap(filename)

    def on_servo_update(self, msg):
        """
    Callback function that handles receipt of a ServoCommand message.
    @param msg: the ServoCommand message
    @type msg: furious.msg.ServoCommand
    """
        if msg.servo == "steering":
            # Use exponential smoothing to smooth out steering values, which
            # often jitter considerably.
            self.steer = int((msg.value * STEER_MIX) + (self.steer *
                                                        (1.0 - STEER_MIX)))

    def on_state_update(self, msg):
        """
    Callback function that handles receipt of a FuriousState message.
    @param msg: the FuriousState message
    @type msg: furious.msg.FuriousState
    """
        self.odometer = msg.odometer
        # TODO: once we actually get some compasses for our robots, then
        # self.compass here.

    def step(self):
        """
    Calculates where the robot is and updates the display.  This method
    is called once on every iteration.
    """
        # How far are we into the current lap?
        lap_distance = self.odometer - self.lap_start
        # What is the probability distribution of our location across all
        # map segments?
        dist = self.map.segment_dist(lap_distance, self.steer, self.compass)
        # Which segment are we most likely to be in?
        most_likely = dist.index(max(dist))
        # Have we moved into a new segment?
        if self.curr_segment != most_likely:
            self.prev_segment = self.curr_segment
            self.curr_segment = most_likely
            # Have we started a new lap?
            if self.curr_segment == 0:
                self.lap_start = self.odometer
        self.draw(dist)

        # Check for user commands
        keypress = self.screen.getch()
        if keypress == ord("q"):  # quit
            rospy.signal_shutdown("User requested shutdown.")

    def draw(self, dist):
        """
    Redraws the screen using the information given.
    @param dist: the probability distribution of the robot's location
    @type dist: list of floats
    """
        s = self.screen
        row = 0
        s.clear()
        s.addstr(row, 0, "===  Oscar map system  ===")
        row += 2
        s.addstr(row, 0, "Odometer:  %.2f" % self.odometer)
        row += 1
        s.addstr(row, 0, "Lap start: %.2f" % self.lap_start)
        row += 1
        s.addstr(row, 0, "Steering:  %4i" % self.steer)
        row += 1
        s.addstr(row, 0, "Compass:   %4i" % self.compass)
        row += 2
        s.addstr(row, 0, "Probability distribution:")
        row += 1
        section = 0
        for p in dist:
            # Show the probability of each section, plus a histogram
            bar = "*" * int(p / 0.05)
            s.addstr(row, 0, "Section %i @ %.2f: %s" % (section, p, bar))
            row += 1
            section += 1
        row += 1
        s.addstr(row, 0, "Commands:")
        row += 1
        s.addstr(row, 2, "(q)uit")
        s.refresh()
예제 #4
0
class MapNode(object):
  def __init__(self, screen):
    self.screen = screen
    self.screen.nodelay(1)  # make screen.getch() non-blocking
    self.curr_segment = None
    self.prev_segment = None
    self.steer = 0
    self.odometer = 0.0
    self.lap_start = 0.0
    self.compass = 0

  def load_mapfile(self, filename):
    self.map = CourseMap(filename)

  def on_servo_update(self, msg):
    """
    Callback function that handles receipt of a ServoCommand message.
    @param msg: the ServoCommand message
    @type msg: furious.msg.ServoCommand
    """
    if msg.servo == "steering":
      # Use exponential smoothing to smooth out steering values, which
      # often jitter considerably.
      self.steer = int( (msg.value * STEER_MIX) + 
          (self.steer * (1.0 - STEER_MIX)) )

  def on_state_update(self, msg):
    """
    Callback function that handles receipt of a FuriousState message.
    @param msg: the FuriousState message
    @type msg: furious.msg.FuriousState
    """
    self.odometer = msg.odometer
    # TODO: once we actually get some compasses for our robots, then
    # self.compass here.

  def step(self):
    """
    Calculates where the robot is and updates the display.  This method
    is called once on every iteration.
    """
    # How far are we into the current lap?
    lap_distance = self.odometer - self.lap_start
    # What is the probability distribution of our location across all
    # map segments?
    dist = self.map.segment_dist(lap_distance, self.steer, self.compass)
    # Which segment are we most likely to be in?
    most_likely = dist.index( max(dist) )
    # Have we moved into a new segment?
    if self.curr_segment != most_likely:
      self.prev_segment = self.curr_segment
      self.curr_segment = most_likely
      # Have we started a new lap?
      if self.curr_segment == 0:
        self.lap_start = self.odometer
    self.draw(dist)

    # Check for user commands
    keypress = self.screen.getch()
    if keypress == ord("q"):   # quit
      rospy.signal_shutdown("User requested shutdown.")

  def draw(self, dist):
    """
    Redraws the screen using the information given.
    @param dist: the probability distribution of the robot's location
    @type dist: list of floats
    """
    s = self.screen
    row = 0
    s.clear()
    s.addstr( row, 0, "===  Oscar map system  ===")
    row += 2
    s.addstr( row, 0, "Odometer:  %.2f" % self.odometer)
    row += 1
    s.addstr( row, 0, "Lap start: %.2f" % self.lap_start)
    row += 1
    s.addstr( row, 0, "Steering:  %4i" % self.steer)
    row += 1
    s.addstr( row, 0, "Compass:   %4i" % self.compass)
    row += 2
    s.addstr( row, 0, "Probability distribution:")
    row += 1
    section = 0
    for p in dist:
      # Show the probability of each section, plus a histogram
      bar = "*" * int(p / 0.05)
      s.addstr( row, 0, "Section %i @ %.2f: %s" % (section, p, bar))
      row += 1
      section += 1
    row += 1
    s.addstr( row, 0, "Commands:" )
    row +=1
    s.addstr( row, 2, "(q)uit")
    s.refresh()