def play(fname, node, tstart, rt_rate, fixed_rate, filter_names=()): p = CHIL.Player(fname) try: tstart_float = float(tstart) p.setCursor(datetime.datetime(year=datetime.MINYEAR, month=1, day=1)) tzero = p.timeOfNextMessage() p.setCursor(tzero + datetime.timedelta(seconds=tstart_float)) except: p.setCursor(datetime.datetime.strptime(tstart, Strptime_Fmt)) if rt_rate is not None: assert(fixed_rate is None) tstart_playback = relativeTime() try: while True: m, td = p.nextMessage() if m is None: break if isinstance(m, str): warning(m) m = None if len(filter_names) and m.__class__.__name__ not in filter_names: continue time_to_sleep_for = tdToFloatSeconds(td) - rt_rate * (relativeTime() - tstart_playback) if time_to_sleep_for/rt_rate > 10: warning('more than 10 seconds until next message will be sent (%gs)' % (time_to_sleep_for/rt_rate)) while time_to_sleep_for > 0: sleep_step = min((time_to_sleep_for/rt_rate, 0.2)) time_to_sleep_for -= sleep_step*rt_rate time.sleep(sleep_step) sys.stdout.write('.'); sys.stdout.flush() if m is not None: if Sonar_Timestamp_Munge and isinstance(m, msg.SonarImageMessage): unix_time = time.mktime(p.cursor().timetuple()) + p.cursor().microsecond/1e6 # for some reason the message seems to be immutable... m = msg.SonarImageMessage( m.source, msg.PolarImage( m.image.data, m.image.encoding, m.image.bearing_bins, m.image.rangeStart, m.image.rangeEnd, m.image.rangeConversion, msg.TimeStamp(int(unix_time), int(1e6*(unix_time-int(unix_time)))) ) ) node.send(m) except Exception, e: error('error in playback: ' + str(e)) raise debug('playback finished')
p_SonarLocationMessage.setParseAction(lambda x: messaging.SonarLocationMessage(*x[0])) p_ImageMessage = pp.Group(l \ + p_CameraID + c \ + p_Image + c \ + p_TimeStamp \ + r).streamline() p_ImageMessage.setParseAction(lambda x: messaging.ImageMessage(*x[0])) p_SonarDataMessage = pp.Group(l \ + p_SonarDataLine \ + r).streamline() p_SonarDataMessage.setParseAction(lambda x: messaging.SonarDataMessage(*x[0])) p_SonarImageMessage = pp.Group(l \ + p_SonarID + c \ + p_PolarImage \ + r).streamline() p_SonarImageMessage.setParseAction(lambda x: messaging.SonarImageMessage(*x[0])) p_SpeedOfSoundMessage = pp.Group(l \ + p_float \ + r).streamline() p_SpeedOfSoundMessage.setParseAction(lambda x: messaging.SpeedOfSoundMessage(*x[0])) p_GeminiStatusMessage = pp.Group(l \ + p_int + c \ + p_int + c \ + p_int + c \ + p_int + c \ + p_GeminiTemperatures + c \ + p_GeminiTemperatures + c \ + p_float + c \ + p_int + c \ + p_int + c \ + p_int \
p_ImageMessage = pp.Group(l \ + p_CameraID + c \ + p_Image + c \ + p_TimeStamp \ + r).streamline() p_ImageMessage.setParseAction(lambda x: messaging.ImageMessage(*x[0])) p_SonarDataMessage = pp.Group(l \ + p_SonarDataLine \ + r).streamline() p_SonarDataMessage.setParseAction(lambda x: messaging.SonarDataMessage(*x[0])) p_SonarImageMessage = pp.Group(l \ + p_SonarID + c \ + p_PolarImage \ + r).streamline() p_SonarImageMessage.setParseAction( lambda x: messaging.SonarImageMessage(*x[0])) p_SpeedOfSoundMessage = pp.Group(l \ + p_float \ + r).streamline() p_SpeedOfSoundMessage.setParseAction( lambda x: messaging.SpeedOfSoundMessage(*x[0])) p_GeminiStatusMessage = pp.Group(l \ + p_int + c \ + p_int + c \ + p_int + c \ + p_int + c \ + p_GeminiTemperatures + c \ + p_GeminiTemperatures + c \ + p_float + c \ + p_int + c \ + p_int + c \
def mainLoop(self): while True: config = self.config() if not config: debug('no gemini config yet!') time.sleep(1) continue time.sleep(config.interPingPeriod) position = self.position() ts = msg.now() if not position: debug('no sim position yet!') continue #struct PolarImage #{ # data: list<byte>; # // currently only raw uint8_t (isodistant responses in fastest varying index) is supported # encoding: ImageEncodingType; # // each bearing in range (-6400 to 6400) * 0x10000, these are the edges of # // the bins that each iso-bearing beam of data falls into, so there are # // num_beams + 1 values. The first bin is bearing_bins[0] -- # // bearing_bins[1], and so on. # // Note that these are *bearings* not angles (ie, clockwise from forwards, not # // anticlockwise from x axis (wherever that is...)) # bearing_bins: list<int32>; # // number of bins = round(rangeEnd-rangeStart) / rangeConversion) # rangeStart: float; # rangeEnd: float; # // rangeConversion = length of 1 bin in metres # rangeConversion: float; # // time of the ping from which the image is acquired # timeStamp : TimeStamp; #} #enum SonarID: int8 { Seasprite = 1, Gemini = 2 } #message SonarImage : 31 #{ # source : SonarID; # image : PolarImage; #} latlong = coordinates.LLACoord.fromWGS84(position.location) beam_bearings = self.gemBeamBearings() bearing_bins = msg.floatVec() for i, a in enumerate(beam_bearings): if i == len(beam_bearings) - 1: prev_a = beam_bearings[i - 1] bearing_bins.append(a + (a - prev_a) / 2) break next_a = beam_bearings[i + 1] if i == 0: bearing_bins.append(a - (next_a - a) / 2) bearing_bins.append(a + (next_a - a) / 2) debug('at:%s =pixels:%s look bearing:%s' % (latlong, self.env.llaToImageCoords(latlong), position.orientation.yaw)) beams = [] for i, a in enumerate(bearing_bins): if i == len(bearing_bins) - 1: break beam_start = a beam_end = bearing_bins[i + 1] assert (beam_end > beam_start) beams.append( self.env.getLine(latlong, position.orientation.yaw + beam_start, position.orientation.yaw + beam_end, config.range, config.rangeLines)) # oops, need to swap order... polar_data = msg.byteVec() for i in xrange(0, len(beams[0])): map(lambda x: polar_data.append(x[i]), beams) # map bearings from floating point bearing into the on-the-wire # integer format (see messages.msg) def mapTo64kRange(v): r = msg.int32Vec() for val in v: i = round(val * 6400 * 0x10000 / 360.0) if i >= 0: i = i % (6400 * 0x10000) else: i = -((-i) % (6400 * 0x10000)) r.append(int(i)) return r img = msg.PolarImage(polar_data, msg.ImageEncodingType.RAW_uint8_1, mapTo64kRange(bearing_bins), 0.0, config.range, config.range / float(config.rangeLines), ts) assert (round( (config.range) / (config.range / float(config.rangeLines))) == len(beams[0])) self.node.send(msg.SonarImageMessage(msg.SonarID.Gemini, img))
m = None if len(filter_names) and m.__class__.__name__ not in filter_names: continue time_to_sleep_for = max(0,(tlast+1.0/fixed_rate)-tlast) time.sleep(time_to_sleep_for) sys.stdout.write('.'); sys.stdout.flush() if m is not None: if Sonar_Timestamp_Munge and isinstance(m, msg.SonarImageMessage): unix_time = time.mktime(p.cursor().timetuple()) + p.cursor().microsecond/1e6 # !!! TODO: for some reason the message seems to be immutable... m = msg.SonarImageMessage( m.source, msg.PolarImage( m.image.data, m.image.encoding, m.image.bearing_bins, m.image.rangeStart, m.image.rangeEnd, m.image.rangeConversion, msg.TimeStamp(int(unix_time), int(1e6*(unix_time-int(unix_time)))) ) ) node.send(m) tlast = relativeTime() except Exception, e: error('error in playback: ' + str(e)) raise debug('playback finished') if __name__ == '__main__': p = argparse.ArgumentParser(description='play a CHIL log file')