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')
