def read_data_for_signals(bag_in, prefix_in, signals):
topic2messages = defaultdict(lambda: dict(timestamp=[], data=[]))
topics = []
for signal_spec in signals:
dtu.check_isinstance(signal_spec, PlotSignalSpec)
topic_fqn = prefix_in + signal_spec.topic
topics.append(topic_fqn)
for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)):
data = interpret_ros(mp.msg)
topic2messages[mp.topic]["data"].append(data)
topic2messages[mp.topic]["timestamp"].append(
mp.time_from_physical_log_start)
for v in list(topic2messages.values()):
v["data"] = np.array(v["data"])
v["timestamp"] = np.array(v["timestamp"])
for signal_spec in signals:
topic_fqn = prefix_in + signal_spec.topic
if not topic_fqn in topic2messages:
msg = f"Could not found any value for topic {topic_fqn!r}."
raise ValueError(msg)
return topic2messages
def analyze_log(self, bag_in, dict_out):
topic2type = look_for_topics(bag_in)
topic2data = defaultdict(lambda: [])
topic2timestamp = defaultdict(lambda: [])
for topic, msg, t in bag_in.read_messages(topics=list(topic2type)):
data = interpret_ros(msg)
timestamp = t.to_sec()
topic2data[topic].append(data)
topic2timestamp[topic].append(timestamp)
for topic in topic2data:
value = np.array(topic2data[topic])
timestamp = np.array(topic2timestamp[topic])
# print topic, value.dtype, value.shape
sane = self.name_from_topic(topic)
dict_out[sane] = compute_stats(timestamp, value)
def do_plot(bag_in, prefix_in, bag_out, prefix_out, signals, plot_name):
topic2messages = defaultdict(lambda: dict(timestamp=[], data=[]))
topics = []
for signal_spec in signals:
dtu.check_isinstance(signal_spec, PlotSignalSpec)
topic_fqn = prefix_in + signal_spec.topic
topics.append(topic_fqn)
for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)):
data = interpret_ros(mp.msg)
topic2messages[mp.topic]["data"].append(data)
topic2messages[mp.topic]["timestamp"].append(
mp.time_from_physical_log_start)
for signal_spec in signals:
topic_fqn = prefix_in + signal_spec.topic
if not topic_fqn in topic2messages:
msg = f"Could not found any value for topic {topic_fqn!r}."
raise ValueError(msg)
bgcolor = dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW
dpi = 100
figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor))
a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args)
use_legend = len(signals) >= 3
# todo: check same units
with a as pylab:
axes = []
_fig, ax0 = pylab.subplots()
ax0.set_xlabel("time (s)")
axes.append(ax0)
if use_legend:
for i in range(len(signals) - 1):
axes.append(ax0)
else:
for i in range(len(signals) - 1):
axes.append(ax0.twinx())
for i, signal_spec in enumerate(signals):
ax = axes[i]
topic_fqn = prefix_in + signal_spec.topic
recorded = topic2messages[topic_fqn]
data = np.array(recorded["data"])
t = np.array(recorded["timestamp"])
color = signal_spec.color
markersize = 5
data_converted = convert_unit(data, signal_spec.units,
signal_spec.units_display)
ax.plot(
t,
data_converted,
"o",
color=color,
label=signal_spec.label,
markersize=markersize,
clip_on=False,
)
if not use_legend:
label = f"{signal_spec.label} [{signal_spec.units_display}]"
ax.set_ylabel(label, color=signal_spec.color)
ax.tick_params("y", colors=color)
ax.set_ylim(signal_spec.min, signal_spec.max)
outward_offset = 20
ax.xaxis.set_tick_params(direction="out")
ax.yaxis.set_tick_params(direction="out")
ax.spines["left"].set_position(("outward", outward_offset))
ax.spines["top"].set_color("none") # don't draw spine
ax.spines["bottom"].set_position(("outward", outward_offset))
ax.spines["right"].set_position(("outward", outward_offset))
pos = {0: "left", 1: "right", 2: "right"}[i]
ax.spines[pos].set_color(color)
ax.xaxis.set_ticks_position("bottom")
if use_legend:
label = f"[{signal_spec.units_display}]"
ax.set_ylabel(label)
pylab.legend()
bgr = a.get_bgr()
plot_name = plot_name.replace("/", "")
# output_filename = os.path.join('tmp', plot_name +'.png')
# dtu.write_bgr_as_jpg(bgr, output_filename)
t_inf = rospy.Time.from_sec(bag_in.get_end_time())
omsg = dru.d8_compressed_image_from_cv_image(bgr, timestamp=t_inf)
otopic = prefix_out + "/" + plot_name
bag_out.write(otopic, omsg, t=t_inf)