4 import matplotlib.pyplot
as plt
8 from dwb_msgs.msg
import LocalPlanEvaluation
16 global fig, rects, max_score
17 labels = [s.name
for s
in msg.twists[msg.best_index].scores]
18 scaled_scores = [s.scale * s.raw_score
for s
in msg.twists[msg.best_index].scores]
22 scaled_scores.reverse()
26 y = np.arange(len(labels))
29 fig, ax = plt.subplots()
30 rects = ax.barh(y - height / 2, scaled_scores, height, label=
'scaled score')
32 ax.set_ylabel(
'DWB Critics')
33 ax.set_title(
'Scaled scores')
35 ax.set_yticklabels(labels)
38 fig.canvas.set_window_title(
'DWB Scores')
41 if max_score < max(scaled_scores):
42 max_score = max(scaled_scores)
43 plt.xlim(0.0, max_score)
45 for rect, h
in zip(rects, scaled_scores):
49 fig.canvas.flush_events()
50 except tkinter.TclError:
51 rospy.loginfo(
'Window was closed, exiting.')
56 rospy.init_node(
'plot_dwb_scores')
57 rospy.Subscriber(
'move_base_node/DWBLocalPlanner/evaluation', LocalPlanEvaluation, eval_cb)
58 rospy.loginfo(
'plot_dwb_scores ready.')
63 if __name__ ==
'__main__':