diff --git a/src/tilde_vis/setup.py b/src/tilde_vis/setup.py index 75f5af3dd5a..238e94bf2f3 100644 --- a/src/tilde_vis/setup.py +++ b/src/tilde_vis/setup.py @@ -1,3 +1,4 @@ +"""Visualization tool for TILDE.""" from setuptools import setup package_name = 'tilde_vis' diff --git a/src/tilde_vis/test/test_data_as_tree.py b/src/tilde_vis/test/test_data_as_tree.py index d6551020106..af47e3fe7f3 100644 --- a/src/tilde_vis/test/test_data_as_tree.py +++ b/src/tilde_vis/test/test_data_as_tree.py @@ -13,14 +13,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest from collections import deque +import unittest from tilde_vis.data_as_tree import TreeNode def dict2tree(adict): - """Convert dictionary to TreeNode. + """ + Convert dictionary to TreeNode. Parameters ---------- @@ -30,10 +31,11 @@ def dict2tree(adict): Return ------ - TreeNode starts from "root" + TreeNode starts from "root". + """ Q = deque() - root = TreeNode("root") + root = TreeNode('root') Q.append((root, adict)) while len(Q) > 0: @@ -50,7 +52,8 @@ def dict2tree(adict): def get_complex_tree(): - """Get complex tree. + """ + Get complex tree. Return ------ @@ -59,20 +62,20 @@ def get_complex_tree(): So, call `ret.get_chile("p1")` and so on to access nodes. """ sample = { - "p1": { - "c1": "c11", - "c2": { - "c21": { - "c211": "c2111", + 'p1': { + 'c1': 'c11', + 'c2': { + 'c21': { + 'c211': 'c2111', }, - "c22": "c221", + 'c22': 'c221', } } } # (TreeNode, sub dictionary) Q = deque() - root = TreeNode("root") + root = TreeNode('root') Q.append((root, sample)) while len(Q) > 0: @@ -90,8 +93,9 @@ def get_complex_tree(): class TestTreeNode(unittest.TestCase): + def test_construct(self): - node = TreeNode("foo") + node = TreeNode('foo') self.assertTrue(node is not None) def test_construct_complex(self): @@ -99,11 +103,11 @@ def test_construct_complex(self): # check tree structure self.assertEqual(len(root.children), 1) - p1 = root.name2child["p1"] + p1 = root.name2child['p1'] self.assertEqual(len(p1.children), 2) - c1 = p1.name2child["c1"] + c1 = p1.name2child['c1'] self.assertEqual(len(c1.children), 1) - c2 = p1.name2child["c2"] + c2 = p1.name2child['c2'] self.assertEqual(len(c2.children), 2) def test_apply(self): @@ -112,52 +116,52 @@ def test_apply(self): self.assertEqual(len(ret), 10) self.assertEqual(ret, - ["root", - "p1", - "c1", - "c11", - "c2", - "c21", - "c211", - "c2111", - "c22", - "c221"]) + ['root', + 'p1', + 'c1', + 'c11', + 'c2', + 'c21', + 'c211', + 'c2111', + 'c22', + 'c221']) def test_apply_with_depth(self): root = get_complex_tree() - ret = root.apply_with_depth(lambda n, depth: f"{n.name}_{depth}") + ret = root.apply_with_depth(lambda n, depth: f'{n.name}_{depth}') self.assertEqual(len(ret), 10) self.assertEqual(ret, - ["root_0", - "p1_1", - "c1_2", - "c11_3", - "c2_2", - "c21_3", - "c211_4", - "c2111_5", - "c22_3", - "c221_4"]) + ['root_0', + 'p1_1', + 'c1_2', + 'c11_3', + 'c2_2', + 'c21_3', + 'c211_4', + 'c2111_5', + 'c22_3', + 'c221_4']) def test_merge(self): lhs_dict = { - "c1": { - "c11": 1, - "c13": 2, + 'c1': { + 'c11': 1, + 'c13': 2, }, - "c2": { - "c21": 3, + 'c2': { + 'c21': 3, }, } rhs_dict = { - "c1": { - "c11": 10, - "c12": { - "c121": 11, + 'c1': { + 'c11': 10, + 'c12': { + 'c121': 11, }, }, - "c2": 12, + 'c2': 12, } lhs = dict2tree(lhs_dict) rhs = dict2tree(rhs_dict) @@ -165,22 +169,22 @@ def test_merge(self): def to_s(tree_node): name = tree_node.name data = tree_node.data - return f"{name}: {data}" + return f'{name}: {data}' lhs.apply(lambda x: print(to_s(x))) lhs.merge(rhs) lhs.apply(lambda x: print(to_s(x))) self.assertEqual(len(lhs.children), 2) - c1 = lhs.get_child("c1") + c1 = lhs.get_child('c1') self.assertEqual(len(c1.children), 3) - self.assertEqual(c1.get_child("c11").data, [1, 10]) - self.assertEqual(c1.get_child("c12").get_child("c121").data, [11]) - self.assertEqual(c1.get_child("c13").data, [2]) + self.assertEqual(c1.get_child('c11').data, [1, 10]) + self.assertEqual(c1.get_child('c12').get_child('c121').data, [11]) + self.assertEqual(c1.get_child('c13').data, [2]) - c2 = lhs.get_child("c2") + c2 = lhs.get_child('c2') self.assertEqual(c2.data, [12]) - self.assertEqual(c2.get_child("c21").data, [3]) + self.assertEqual(c2.get_child('c21').data, [3]) if __name__ == '__main__': diff --git a/src/tilde_vis/test/test_latency_viewer.py b/src/tilde_vis/test/test_latency_viewer.py index c39c4848c76..35370df018a 100644 --- a/src/tilde_vis/test/test_latency_viewer.py +++ b/src/tilde_vis/test/test_latency_viewer.py @@ -15,50 +15,51 @@ import unittest +from builtin_interfaces.msg import Time as TimeMsg import rclpy from rclpy.time import Time -from builtin_interfaces.msg import Time as TimeMsg -from tilde_vis.data_as_tree import TreeNode -from tilde_vis.pub_info import ( - PubInfo +from tilde_msg.msg import ( + PubInfo as PubInfoMsg, ) +from tilde_vis.data_as_tree import TreeNode from tilde_vis.latency_viewer import ( calc_stat, - update_stat, LatencyViewerNode, + update_stat, ) -from tilde_msg.msg import ( - PubInfo as PubInfoMsg, +from tilde_vis.pub_info import ( + PubInfo ) class TestCalcStat(unittest.TestCase): + def test_none(self): - root = TreeNode("root") + root = TreeNode('root') root.data = [(None, None), (None, None), (None, None)] ret = calc_stat(root) self.assertEqual(len(ret), 1) - self.assertEqual(ret[0]["dur_min"], None) - self.assertEqual(ret[0]["dur_mean"], None) + self.assertEqual(ret[0]['dur_min'], None) + self.assertEqual(ret[0]['dur_mean'], None) def test_simple(self): - root = TreeNode("root") + root = TreeNode('root') root.data = [(1, 10), (2, 20), (3, 30)] ret = calc_stat(root) self.assertEqual(len(ret), 1) - self.assertEqual(ret[0]["dur_min"], 1) - self.assertEqual(ret[0]["dur_mean"], 2) - self.assertEqual(ret[0]["dur_max"], 3) - self.assertEqual(ret[0]["dur_min_steady"], 10) - self.assertEqual(ret[0]["dur_mean_steady"], 20) - self.assertEqual(ret[0]["dur_max_steady"], 30) + self.assertEqual(ret[0]['dur_min'], 1) + self.assertEqual(ret[0]['dur_mean'], 2) + self.assertEqual(ret[0]['dur_max'], 3) + self.assertEqual(ret[0]['dur_min_steady'], 10) + self.assertEqual(ret[0]['dur_mean_steady'], 20) + self.assertEqual(ret[0]['dur_max_steady'], 30) def time_msg(sec, ms): - """Get builtin.msg.Time""" + """Get builtin.msg.Time.""" return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() @@ -66,7 +67,9 @@ def get_solver_result_simple( t1_pub, t1_sub, t2_pub, t2_sub, t3_pub): - """Generate solver result + """ + Generate solver result. + Graph: topic1 --> topic2 --> topic3 You can specify each pub/sub time. @@ -74,29 +77,29 @@ def get_solver_result_simple( - header_stamp is preserved at topic1 pub time. - stamp and stamp_steady are same """ - tree_node3 = TreeNode("topic3") - tree_node2 = tree_node3.get_child("topic2") - tree_node1 = tree_node2.get_child("topic1") + tree_node3 = TreeNode('topic3') + tree_node2 = tree_node3.get_child('topic2') + tree_node1 = tree_node2.get_child('topic1') stamp1 = t1_pub pubinfo3 = PubInfo( - "topic3", + 'topic3', t3_pub, t3_pub, True, stamp1) pubinfo3.add_input_info( - "topic2", t2_sub, t2_sub, + 'topic2', t2_sub, t2_sub, True, stamp1) tree_node3.add_data(pubinfo3) pubinfo2 = PubInfo( - "topic2", + 'topic2', t2_pub, t2_pub, True, stamp1) pubinfo2.add_input_info( - "topic1", t1_sub, t1_sub, + 'topic1', t1_sub, t1_sub, True, stamp1) tree_node2.add_data(pubinfo2) pubinfo1 = PubInfo( - "topic1", + 'topic1', t1_pub, t1_pub, True, stamp1) tree_node1.add_data(pubinfo1) @@ -104,8 +107,11 @@ def get_solver_result_simple( class TestUpdateStat(unittest.TestCase): + def test_simple(self): """ + Simple case. + pub/sub time (case1) 10 11 12 13 14 (case2) 20 24 28 30 35 @@ -124,15 +130,16 @@ def test_simple(self): topic3_result_data = update_stat_case1.data self.assertEqual(topic3_result_data[0], (0, 0)) - topic2_result_data = update_stat_case1.name2child["topic2"].data + topic2_result_data = update_stat_case1.name2child['topic2'].data self.assertEqual(topic2_result_data[0], (2, 2)) topic1_result_data = \ - update_stat_case1.name2child["topic2"].name2child["topic1"].data + update_stat_case1.name2child['topic2'].name2child['topic1'].data self.assertEqual(topic1_result_data[0], (4, 4)) class TestListenerCallback(unittest.TestCase): + @classmethod def setUpClass(cls): rclpy.init() @@ -142,7 +149,7 @@ def tearDownClass(cls): rclpy.shutdown() def test_seq(self): - topic_name = "topic" + topic_name = 'topic' node = LatencyViewerNode() msg = PubInfoMsg() msg.output_info.topic_name = topic_name @@ -175,7 +182,7 @@ def test_seq(self): node.destroy_node() def test_seq_non_zero_start(self): - topic_name = "topic" + topic_name = 'topic' node = LatencyViewerNode() msg = PubInfoMsg() msg.output_info.topic_name = topic_name @@ -202,6 +209,7 @@ def test_seq_non_zero_start(self): class TestTimerCallback(unittest.TestCase): + def setUp(self): rclpy.init() @@ -209,7 +217,7 @@ def tearDown(self): rclpy.shutdown() def test_issues23_1(self): - topic_name = "topic" + topic_name = 'topic' node = LatencyViewerNode() node.target_topic = topic_name @@ -231,10 +239,10 @@ def test_issues23_1(self): node.timer_callback() self.assertTrue(True) except AttributeError: - self.fail(msg="timer_callback causes AttributeError") + self.fail(msg='timer_callback causes AttributeError') def test_issues23_2(self): - topic_name = "topic" + topic_name = 'topic' node = LatencyViewerNode() node.target_topic = topic_name @@ -257,7 +265,7 @@ def test_issues23_2(self): node.timer_callback() self.assertTrue(True) except AttributeError: - self.fail(msg="timer_callback causes AttributeError") + self.fail(msg='timer_callback causes AttributeError') if __name__ == '__main__': diff --git a/src/tilde_vis/test/test_ncurse_printer.py b/src/tilde_vis/test/test_ncurse_printer.py index 807bd93b5bb..c111dc344b2 100644 --- a/src/tilde_vis/test/test_ncurse_printer.py +++ b/src/tilde_vis/test/test_ncurse_printer.py @@ -26,8 +26,8 @@ def main(stdscr, args=None): printer = NcursesPrinter(stdscr) while True: now = time.time() - lines = [f"{now}: {i}" for i in range(100)] - printer.print(lines) + lines = [f'{now}: {i}' for i in range(100)] + printer.print_(lines) time.sleep(1) diff --git a/src/tilde_vis/test/test_pub_info.py b/src/tilde_vis/test/test_pub_info.py index 090bf827cd2..2a0512a5a87 100644 --- a/src/tilde_vis/test/test_pub_info.py +++ b/src/tilde_vis/test/test_pub_info.py @@ -24,29 +24,30 @@ def time_msg(sec, ms): - """Get builtin.msg.Time""" + """Get builtin.msg.Time.""" return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() class TestPubInfos(unittest.TestCase): + def test_erase_until(self): infos = PubInfos() infos.add(PubInfo( - "topic1", + 'topic1', time_msg(8, 0), time_msg(9, 0), True, time_msg(10, 0) )) infos.add(PubInfo( - "topic2", + 'topic2', time_msg(18, 0), time_msg(19, 0), True, time_msg(20, 0) )) infos.add(PubInfo( - "topic1", + 'topic1', time_msg(28, 0), time_msg(29, 0), True, @@ -54,37 +55,37 @@ def test_erase_until(self): )) self.assertEqual(len(infos.topic_vs_pubinfos.keys()), 2) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 2) - self.assertEqual(len(infos.topic_vs_pubinfos["topic2"].keys()), 1) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_pubinfos['topic2'].keys()), 1) infos.erase_until(time_msg(9, 999)) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 2) - self.assertEqual(len(infos.topic_vs_pubinfos["topic2"].keys()), 1) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_pubinfos['topic2'].keys()), 1) # boundary condition - not deleted infos.erase_until(time_msg(10, 0)) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 2) - self.assertEqual(len(infos.topic_vs_pubinfos["topic2"].keys()), 1) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_pubinfos['topic2'].keys()), 1) infos.erase_until(time_msg(10, 1)) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 1) - self.assertTrue("30.000000000" in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertEqual(len(infos.topic_vs_pubinfos["topic2"].keys()), 1) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 1) + self.assertTrue('30.000000000' in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertEqual(len(infos.topic_vs_pubinfos['topic2'].keys()), 1) # topic2 at t=30.0 is deleted, but key is preserved infos.erase_until(time_msg(20, 1)) self.assertEqual(len(infos.topic_vs_pubinfos.keys()), 2) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 1) - self.assertTrue("30.000000000" in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertEqual(len(infos.topic_vs_pubinfos["topic2"]), 0) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 1) + self.assertTrue('30.000000000' in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertEqual(len(infos.topic_vs_pubinfos['topic2']), 0) def test_erase_until_many(self): infos = PubInfos() for i in range(0, 10): infos.add(PubInfo( - "topic1", + 'topic1', time_msg(i, 0), time_msg(i, 0), True, @@ -92,26 +93,26 @@ def test_erase_until_many(self): )) infos.erase_until(time_msg(3, 1)) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 6) - self.assertTrue("0.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("1.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("2.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("3.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 6) + self.assertTrue('0.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('1.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('2.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('3.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) infos.erase_until(time_msg(7, 1)) - self.assertEqual(len(infos.topic_vs_pubinfos["topic1"].keys()), 2) - self.assertTrue("4.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("5.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("6.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) - self.assertTrue("7.000000000" not in - infos.topic_vs_pubinfos["topic1"].keys()) + self.assertEqual(len(infos.topic_vs_pubinfos['topic1'].keys()), 2) + self.assertTrue('4.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('5.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('6.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) + self.assertTrue('7.000000000' not in + infos.topic_vs_pubinfos['topic1'].keys()) if __name__ == '__main__': diff --git a/src/tilde_vis/test/test_pubinfo_traverse.py b/src/tilde_vis/test/test_pubinfo_traverse.py index 4a61ff8e04d..96948cbe687 100644 --- a/src/tilde_vis/test/test_pubinfo_traverse.py +++ b/src/tilde_vis/test/test_pubinfo_traverse.py @@ -16,22 +16,23 @@ import unittest from rclpy.clock import ClockType -from rclpy.time import Time from rclpy.duration import Duration +from rclpy.time import Time +from tilde_vis.latency_viewer import ( + calc_one_hot, + update_stat, + ) from tilde_vis.pub_info import PubInfo, PubInfos from tilde_vis.pubinfo_traverse import ( InputSensorStampSolver, TopicGraph, ) -from tilde_vis.latency_viewer import ( - calc_one_hot, - update_stat, - ) def get_scenario1(): - """Get the following scenario + """ + Get the following scenario. topic1 --> topic2 --> topic3 @@ -47,10 +48,10 @@ def get_topic2_times(start_time, start_time_steady): sub_time_steady = start_time_steady + nw_dur pub_time_steady = sub_time_steady + cb_dur - return {"sub": sub_time, - "pub": pub_time, - "sub_steady": sub_time_steady, - "pub_steady": pub_time_steady} + return {'sub': sub_time, + 'pub': pub_time, + 'sub_steady': sub_time_steady, + 'pub_steady': pub_time_steady} def get_topic3_times(start_time, start_time_steady): sub_time = start_time + nw_dur + cb_dur + nw_dur @@ -59,10 +60,10 @@ def get_topic3_times(start_time, start_time_steady): sub_time_steady = start_time_steady + nw_dur + cb_dur + nw_dur pub_time_steady = sub_time_steady + cb_dur - return {"sub": sub_time, - "pub": pub_time, - "sub_steady": sub_time_steady, - "pub_steady": pub_time_steady} + return {'sub': sub_time, + 'pub': pub_time, + 'sub_steady': sub_time_steady, + 'pub_steady': pub_time_steady} t1 = Time(seconds=10, nanoseconds=0) t1_steady = Time(seconds=0, nanoseconds=1, @@ -71,7 +72,7 @@ def get_topic3_times(start_time, start_time_steady): cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] pubinfo_topic1_t1 = \ - PubInfo("topic1", + PubInfo('topic1', t1.to_msg(), t1_steady.to_msg(), True, @@ -79,29 +80,29 @@ def get_topic3_times(start_time, start_time_steady): topic2_t1_times = get_topic2_times(t1, t1_steady) pubinfo_topic2_t1 = \ - PubInfo("topic2", - topic2_t1_times["pub"].to_msg(), - topic2_t1_times["pub_steady"].to_msg(), + PubInfo('topic2', + topic2_t1_times['pub'].to_msg(), + topic2_t1_times['pub_steady'].to_msg(), True, t1.to_msg()) pubinfo_topic2_t1.add_input_info( - "topic1", - topic2_t1_times["sub"], - topic2_t1_times["sub_steady"], + 'topic1', + topic2_t1_times['sub'], + topic2_t1_times['sub_steady'], True, t1.to_msg()) topic3_t1_times = get_topic3_times(t1, t1_steady) pubinfo_topic3_t1 = \ - PubInfo("topic3", - topic3_t1_times["pub"].to_msg(), - topic3_t1_times["pub_steady"].to_msg(), + PubInfo('topic3', + topic3_t1_times['pub'].to_msg(), + topic3_t1_times['pub_steady'].to_msg(), True, t1.to_msg()) pubinfo_topic3_t1.add_input_info( - "topic2", - topic3_t1_times["sub"], - topic3_t1_times["sub_steady"], + 'topic2', + topic3_t1_times['sub'], + topic3_t1_times['sub_steady'], True, t1.to_msg()) @@ -111,16 +112,15 @@ def get_topic3_times(start_time, start_time_steady): def dur_plus(lhs, rhs): - """ - plus of Duration - """ + """Plus of Duration.""" return Duration(nanoseconds=lhs.nanoseconds + rhs.nanoseconds) def gen_scenario2( st, st_steady, nw_dur, cb_dur): - """Get the following scenario + """ + Get the following scenario. topic1 --> topic3 --> topic4 topic2 ---/ @@ -136,9 +136,10 @@ def gen_scenario2( ------- list of PubInfos such as (pubinfo of topic1, pubinfo of topic2, ...) + """ pubinfo_topic1 = \ - PubInfo("topic1", + PubInfo('topic1', st.to_msg(), st_steady.to_msg(), True, @@ -146,7 +147,7 @@ def gen_scenario2( topic2_stamp = st + nw_dur pubinfo_topic2 = \ - PubInfo("topic2", + PubInfo('topic2', st.to_msg(), st_steady.to_msg(), True, @@ -156,20 +157,20 @@ def gen_scenario2( pub_dur3 = dur_plus(sub_dur3, cb_dur) topic3_stamp = st + pub_dur3 pubinfo_topic3 = \ - PubInfo("topic3", + PubInfo('topic3', (st + pub_dur3).to_msg(), (st_steady + pub_dur3).to_msg(), True, topic3_stamp.to_msg()) pubinfo_topic3.add_input_info( - "topic1", + 'topic1', (st + sub_dur3).to_msg(), (st_steady + sub_dur3).to_msg(), True, st.to_msg() ) pubinfo_topic3.add_input_info( - "topic2", + 'topic2', (st + sub_dur3).to_msg(), (st_steady + sub_dur3).to_msg(), True, @@ -179,13 +180,13 @@ def gen_scenario2( pub_dur4 = dur_plus(sub_dur4, cb_dur) topic4_stamp = st + pub_dur4 pubinfo_topic4 = \ - PubInfo("topic4", + PubInfo('topic4', (st + pub_dur4).to_msg(), (st_steady + pub_dur4).to_msg(), True, topic4_stamp.to_msg()) pubinfo_topic4.add_input_info( - "topic3", + 'topic3', (st + sub_dur4).to_msg(), (st_steady + sub_dur4).to_msg(), True, @@ -200,6 +201,7 @@ def gen_scenario2( class TestTopicGraph(unittest.TestCase): + def test_straight(self): infos = get_scenario1() pubinfos = PubInfos() @@ -208,19 +210,19 @@ def test_straight(self): graph = TopicGraph(pubinfos, skips={}) - self.assertEqual(graph.topics[0], "topic1") - self.assertEqual(graph.topics[1], "topic2") - self.assertEqual(graph.topics[2], "topic3") + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') edges = graph.topic_edges - self.assertEqual(edges[0], set([1])) - self.assertEqual(edges[1], set([2])) + self.assertEqual(edges[0], {1}) + self.assertEqual(edges[1], {2}) self.assertEqual(edges[2], set()) rev_edges = graph.rev_edges self.assertEqual(rev_edges[0], set()) - self.assertEqual(rev_edges[1], set([0])) - self.assertEqual(rev_edges[2], set([1])) + self.assertEqual(rev_edges[1], {0}) + self.assertEqual(rev_edges[2], {1}) def test_scenario2(self): t0 = Time(seconds=10, nanoseconds=0) @@ -236,25 +238,27 @@ def test_scenario2(self): graph = TopicGraph(pubinfos, skips={}) - self.assertEqual(graph.topics[0], "topic1") - self.assertEqual(graph.topics[1], "topic2") - self.assertEqual(graph.topics[2], "topic3") - self.assertEqual(graph.topics[3], "topic4") + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') + self.assertEqual(graph.topics[3], 'topic4') edges = graph.topic_edges - self.assertEqual(edges[0], set([2])) - self.assertEqual(edges[1], set([2])) - self.assertEqual(edges[2], set([3])) + self.assertEqual(edges[0], {2}) + self.assertEqual(edges[1], {2}) + self.assertEqual(edges[2], {3}) self.assertEqual(edges[3], set()) rev_edges = graph.rev_edges self.assertEqual(rev_edges[0], set()) self.assertEqual(rev_edges[1], set()) - self.assertEqual(rev_edges[2], set([0, 1])) - self.assertEqual(rev_edges[3], set([2])) + self.assertEqual(rev_edges[2], {0, 1}) + self.assertEqual(rev_edges[3], {2}) def test_scenario2_with_loss(self): - """Graph creation with lossy PubInfos + """ + Graph creation with lossy PubInfos. + t0: pubinfo only topic1 and topic3 t1: only topic2 t2: only topic4 @@ -280,25 +284,26 @@ def test_scenario2_with_loss(self): graph = TopicGraph(pubinfos, skips={}) - self.assertEqual(graph.topics[0], "topic1") - self.assertEqual(graph.topics[1], "topic2") - self.assertEqual(graph.topics[2], "topic3") - self.assertEqual(graph.topics[3], "topic4") + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') + self.assertEqual(graph.topics[3], 'topic4') edges = graph.topic_edges - self.assertEqual(edges[0], set([2])) - self.assertEqual(edges[1], set([2])) - self.assertEqual(edges[2], set([3])) + self.assertEqual(edges[0], {2}) + self.assertEqual(edges[1], {2}) + self.assertEqual(edges[2], {3}) self.assertEqual(edges[3], set()) rev_edges = graph.rev_edges self.assertEqual(rev_edges[0], set()) self.assertEqual(rev_edges[1], set()) - self.assertEqual(rev_edges[2], set([0, 1])) - self.assertEqual(rev_edges[3], set([2])) + self.assertEqual(rev_edges[2], {0, 1}) + self.assertEqual(rev_edges[3], {2}) class TestTreeNode(unittest.TestCase): + def test_solve2_straight(self): infos = get_scenario1() pubinfos = PubInfos() @@ -307,27 +312,27 @@ def test_solve2_straight(self): graph = TopicGraph(pubinfos, skips={}) solver = InputSensorStampSolver(graph) - tgt_stamp = sorted(pubinfos.stamps("topic3"))[0] + tgt_stamp = sorted(pubinfos.stamps('topic3'))[0] results = solver.solve2( pubinfos, - "topic3", + 'topic3', tgt_stamp ) r3 = results - self.assertEqual(r3.name, "topic3") + self.assertEqual(r3.name, 'topic3') self.assertEqual(len(r3.data), 1) - self.assertEqual(len(r3.data[0].in_infos["topic2"]), 1) - self.assertEqual(r3.data[0].in_infos["topic2"][0].stamp.sec, 10) + self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) + self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp.sec, 10) - r2 = r3.name2child["topic2"] - self.assertEqual(r2.name, "topic2") + r2 = r3.name2child['topic2'] + self.assertEqual(r2.name, 'topic2') self.assertEqual(len(r2.data), 1) - self.assertEqual(len(r2.data[0].in_infos["topic1"]), 1) - self.assertEqual(r2.data[0].in_infos["topic1"][0].stamp.sec, 10) + self.assertEqual(len(r2.data[0].in_infos['topic1']), 1) + self.assertEqual(r2.data[0].in_infos['topic1'][0].stamp.sec, 10) - r1 = r2.name2child["topic1"] - self.assertEqual(r1.name, "topic1") + r1 = r2.name2child['topic1'] + self.assertEqual(r1.name, 'topic1') self.assertEqual(len(r1.data), 1) self.assertEqual(len(r1.data[0].in_infos.keys()), 0) @@ -358,12 +363,12 @@ def test_solve2_branched(self): graph = TopicGraph(pubinfos, skips={}) solver = InputSensorStampSolver(graph) - sorted_stamps = sorted(pubinfos.stamps("topic4")) + sorted_stamps = sorted(pubinfos.stamps('topic4')) tgt_stamp = sorted_stamps[75] results = solver.solve2( pubinfos, - "topic4", + 'topic4', tgt_stamp) test_stamp1 = test_st @@ -372,44 +377,44 @@ def test_solve2_branched(self): test_stamp4 = test_stamp3 + nw_dur + cb_dur r4 = results - self.assertEqual(r4.name, "topic4") + self.assertEqual(r4.name, 'topic4') self.assertEqual(r4.data[0].out_info.stamp, test_stamp4.to_msg()) self.assertEqual(len(r4.data), 1) - self.assertEqual(len(r4.data[0].in_infos["topic3"]), 1) - self.assertEqual(r4.data[0].in_infos["topic3"][0].stamp, + self.assertEqual(len(r4.data[0].in_infos['topic3']), 1) + self.assertEqual(r4.data[0].in_infos['topic3'][0].stamp, test_stamp3.to_msg()) - r3 = r4.name2child["topic3"] - self.assertEqual(r3.name, "topic3") + r3 = r4.name2child['topic3'] + self.assertEqual(r3.name, 'topic3') self.assertEqual(len(r3.data), 1) - self.assertEqual(len(r3.data[0].in_infos["topic2"]), 1) - self.assertEqual(r3.data[0].in_infos["topic2"][0].stamp, + self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) + self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp, test_stamp2.to_msg()) - self.assertEqual(len(r3.data[0].in_infos["topic1"]), 1) - self.assertEqual(r3.data[0].in_infos["topic1"][0].stamp, + self.assertEqual(len(r3.data[0].in_infos['topic1']), 1) + self.assertEqual(r3.data[0].in_infos['topic1'][0].stamp, test_stamp1.to_msg()) onehot_durs = calc_one_hot(results) self.assertEqual(len(onehot_durs), 4) dur4 = onehot_durs[0] - self.assertEqual(dur4[1], "topic4") + self.assertEqual(dur4[1], 'topic4') self.assertEqual(dur4[3], 0) dur3 = onehot_durs[1] - self.assertEqual(dur3[1], "topic3") + self.assertEqual(dur3[1], 'topic3') self.assertEqual(dur3[3], 11) dur2 = onehot_durs[3] - self.assertEqual(dur2[1], "topic2") + self.assertEqual(dur2[1], 'topic2') self.assertEqual(dur2[3], 22) dur1 = onehot_durs[2] - self.assertEqual(dur1[1], "topic1") + self.assertEqual(dur1[1], 'topic1') self.assertEqual(dur1[3], 22) def setup_only_topic4_scenario(self): - """setup lossy scenario""" + """Test lossy scenario.""" t0 = Time(seconds=10, nanoseconds=0) t0_steady = Time(seconds=0, nanoseconds=1, clock_type=ClockType.STEADY_TIME) @@ -442,32 +447,31 @@ def get_pubinfos_with_only_topic4(): def test_solve_empty(self): # setup solver, _ = self.setup_only_topic4_scenario() - tgt_topic = "topic4" + tgt_topic = 'topic4' results = solver._solve_empty(tgt_topic) result_topic4 = results - self.assertEqual(result_topic4.name, "topic4") + self.assertEqual(result_topic4.name, 'topic4') self.assertEqual(len(result_topic4.data), 0) - result_topic3 = result_topic4.name2child["topic3"] - self.assertEqual(result_topic3.name, "topic3") + result_topic3 = result_topic4.name2child['topic3'] + self.assertEqual(result_topic3.name, 'topic3') self.assertEqual(len(result_topic3.data), 0) - result_topic2 = result_topic3 .name2child["topic2"] - self.assertEqual(result_topic2.name, "topic2") + result_topic2 = result_topic3 .name2child['topic2'] + self.assertEqual(result_topic2.name, 'topic2') self.assertEqual(len(result_topic2.data), 0) - result_topic1 = result_topic3.name2child["topic1"] - self.assertEqual(result_topic1.name, "topic1") + result_topic1 = result_topic3.name2child['topic1'] + self.assertEqual(result_topic1.name, 'topic1') self.assertEqual(len(result_topic1.data), 0) def test_solve2_with_loss(self): - """Solve with lossy PubInfos - """ + """Solve with lossy PubInfos.""" solver, tgt_pubinfos = self.setup_only_topic4_scenario() - tgt_topic = "topic4" + tgt_topic = 'topic4' tgt_stamps = tgt_pubinfos.stamps(tgt_topic) self.assertEqual(len(tgt_stamps), 1) @@ -475,20 +479,20 @@ def test_solve2_with_loss(self): # is there full graph? result_topic4 = results - self.assertEqual(result_topic4.name, "topic4") + self.assertEqual(result_topic4.name, 'topic4') self.assertEqual(len(result_topic4.data), 1) self.assertTrue(isinstance(result_topic4.data[0], PubInfo)) - result_topic3 = result_topic4.name2child["topic3"] - self.assertEqual(result_topic3.name, "topic3") + result_topic3 = result_topic4.name2child['topic3'] + self.assertEqual(result_topic3.name, 'topic3') self.assertEqual(len(result_topic3.data), 0) - result_topic2 = result_topic3.name2child["topic2"] - self.assertEqual(result_topic2.name, "topic2") + result_topic2 = result_topic3.name2child['topic2'] + self.assertEqual(result_topic2.name, 'topic2') self.assertEqual(len(result_topic2.data), 0) - result_topic1 = result_topic3.name2child["topic1"] - self.assertEqual(result_topic1.name, "topic1") + result_topic1 = result_topic3.name2child['topic1'] + self.assertEqual(result_topic1.name, 'topic1') self.assertEqual(len(result_topic1.data), 0) def test_update_stat(self): @@ -526,21 +530,21 @@ def test_update_stat(self): graph = TopicGraph(pubinfos, skips={}) solver = InputSensorStampSolver(graph) - tgt_stamp = sorted(pubinfos.stamps("topic4"))[-1] + tgt_stamp = sorted(pubinfos.stamps('topic4'))[-1] results = solver.solve2( pubinfos, - "topic4", + 'topic4', tgt_stamp) results = update_stat(results) self.assertEqual(results.data, [(0, 0)]) - topic3 = results.get_child("topic3") + topic3 = results.get_child('topic3') self.assertEqual(topic3.data, [(33, 33)]) - topic2 = topic3.get_child("topic2") + topic2 = topic3.get_child('topic2') self.assertEqual(topic2.data, [(66, 66)]) - topic1 = topic3.get_child("topic1") + topic1 = topic3.get_child('topic1') self.assertEqual(topic1.data, [(66, 66)]) diff --git a/src/tilde_vis/tilde_vis/__init__.py b/src/tilde_vis/tilde_vis/__init__.py index e69de29bb2d..141c8594d91 100644 --- a/src/tilde_vis/tilde_vis/__init__.py +++ b/src/tilde_vis/tilde_vis/__init__.py @@ -0,0 +1,15 @@ +# Copyright 2021 Research Institute of Systems Planning, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Package tilde_vis.""" diff --git a/src/tilde_vis/tilde_vis/caret_vis_tilde.py b/src/tilde_vis/tilde_vis/caret_vis_tilde.py index 01028c33e03..6e0e885b96a 100755 --- a/src/tilde_vis/tilde_vis/caret_vis_tilde.py +++ b/src/tilde_vis/tilde_vis/caret_vis_tilde.py @@ -13,16 +13,18 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Check CARET vs TILDE results.""" + from collections import defaultdict -from bokeh.plotting import figure, show from bokeh.models import CrosshairTool -import pandas as pd +from bokeh.plotting import figure, show import numpy as np +import pandas as pd from tilde_vis.pub_info import ( - PubInfos, - PubInfo + PubInfo, + PubInfos ) from tilde_vis.pubinfo_traverse import ( InputSensorStampSolver, @@ -31,16 +33,12 @@ def time2str(t): - """ - t: builtin_interfaces.msg.Time - """ - return f"{t.sec}.{t.nanosec:09d}" + """Convert builtin_interfaces.msg.Time to string.""" + return f'{t.sec}.{t.nanosec:09d}' def time2int(t): - """ - t: builtin_interfaces.msg.Time - """ + """Convert builtin_interfaces.msg.Time to int.""" return t.sec * 10**9 + t.nanosec @@ -69,7 +67,7 @@ def _rosbag_iter(rosbag_path): def read_msgs(rosbag_path): """ - Read demo messages from rosbag + Read demo messages from rosbag. Returns ------- @@ -77,6 +75,7 @@ def read_msgs(rosbag_path): { : [] } + """ msg_topics = defaultdict(list) for topic, msg, t in _rosbag_iter(rosbag_path): @@ -85,15 +84,17 @@ def read_msgs(rosbag_path): def read_pubinfo(raw_msgs): + """Convert raw messages to PubInfos.""" pubinfos = PubInfos() for i in range(5): - msgs = raw_msgs[f"/topic{i+1}/info/pub"] + msgs = raw_msgs[f'/topic{i+1}/info/pub'] for msg in msgs: pubinfos.add(PubInfo.fromMsg(msg)) return pubinfos def get_uuid2msg(raw_msgs): + """Convert raw messages to dictionary.""" hashed_msgs = {} hash_target_keys = ['/topic1', '/topic2', '/topic3', '/topic4', '/topic5'] for hash_target_key in hash_target_keys: @@ -105,7 +106,13 @@ def get_uuid2msg(raw_msgs): def build_latency_table(traces, ds): """ + Calculate latencies. + + Parameters + ---------- + traces: CARET traces ds: list to output + """ traces_dict = {trace.uuid: trace for trace in traces} @@ -134,7 +141,7 @@ def search_local(uuid, flow): # start_msgs = msgs_topics['/topic1'] start_msgs = [trace.uuid for trace in traces - if "/topic1" in trace.uuid and trace.trace_type == "publish"] + if '/topic1' in trace.uuid and trace.trace_type == 'publish'] uid_flows = [] for msg in start_msgs: @@ -163,23 +170,28 @@ def trace_to_column(trace): class Trace(object): + """Trace data for CARET.""" + def __init__(self, node_name, uuid, stamp, is_publish, uuids): + """Constructor.""" self.node_name = node_name # "" self.uuid = uuid # "_" self.steady_t = stamp - self.trace_type = "publish" if is_publish else "callback_start" + self.trace_type = 'publish' if is_publish else 'callback_start' self.used_uuids = uuids def __repr__(self): + """Print self.""" return ( ' ' - f"node_name={self.node_name} uuid={self.uuid} " - f"steady_t={self.steady_t} trace_type={self.trace_type} " - f"used_uuids={self.used_uuids}") + f'node_name={self.node_name} uuid={self.uuid} ' + f'steady_t={self.steady_t} trace_type={self.trace_type} ' + f'used_uuids={self.used_uuids}') def vis_tilde(pub_infos): - tgt_topic = "/topic5" + """Visualize tilde result.""" + tgt_topic = '/topic5' topic5_stamps = pub_infos.stamps(tgt_topic) graph = TopicGraph(pub_infos) @@ -196,13 +208,13 @@ def update_traces(node): for d in node.data: stamp = time2int(d.out_info.stamp) pubtime = time2int(d.out_info.pubsub_stamp) - uuid = f"{topic}_{stamp}" + uuid = f'{topic}_{stamp}' uuids = [] for in_topic, infos in d.in_infos.items(): for i in infos: i_stamp = time2int(i.stamp) subtime = time2int(i.pubsub_stamp) - i_uuid = f"{in_topic}_{i_stamp}" + i_uuid = f'{in_topic}_{i_stamp}' uuids.append(i_uuid) # append callback_start @@ -219,7 +231,7 @@ def update_traces(node): def plot_latency_table(df): - # dfからフローの可視化 + """Plot flows from df.""" from tqdm import tqdm from bokeh.palettes import Bokeh8 @@ -248,7 +260,8 @@ def get_color(i): def main(): - bagfile = "rosbag2_2022_02_16-18_12_46" + """Main.""" + bagfile = 'rosbag2_2022_02_16-18_12_46' raw_msgs = read_msgs(bagfile) pub_infos = read_pubinfo(raw_msgs) ds = vis_tilde(pub_infos) @@ -257,5 +270,6 @@ def main(): plot_latency_table(df) -if __name__ == "__main__": +if __name__ == '__main__': + """Main.""" main() diff --git a/src/tilde_vis/tilde_vis/data_as_tree.py b/src/tilde_vis/tilde_vis/data_as_tree.py index 14b39b8519c..05b36d98472 100644 --- a/src/tilde_vis/tilde_vis/data_as_tree.py +++ b/src/tilde_vis/tilde_vis/data_as_tree.py @@ -12,12 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Tree like data structure for PubInfo traversal.""" + class TreeNode(object): - """Node of DataAsTree. + """ + Node of DataAsTree. + This hold data list and children. + """ + def __init__(self, name): + """Constructor.""" self.name = name self.children = [] self.name2child = {} @@ -25,11 +32,15 @@ def __init__(self, name): self.value = None def num_children(self): + """Get the number of children.""" return len(self.children) def get_child(self, child_name): - """ Get child. + """ + Get child. + If not exists, then append. + """ children = self.children name2child = self.name2child @@ -45,10 +56,20 @@ def get_child(self, child_name): return c def add_data(self, d): + """ + Add data. + + Parameters + ---------- + d: any value or object + + """ self.data.append(d) def apply(self, fn): - """Apply fn recursively. + """ + Apply fn recursively. + Internally, preordering DFS is used. Parameters @@ -58,6 +79,7 @@ def apply(self, fn): Return ------ list of fn return + """ children = self.children ret = [] @@ -69,7 +91,9 @@ def apply(self, fn): return ret def apply_with_depth(self, fn, depth=0): - """Apply with depth option + """ + Apply with depth option. + Internally, preordering DFS is used. Parameters @@ -79,6 +103,7 @@ def apply_with_depth(self, fn, depth=0): Return ------ list of fn return + """ children = self.children ret = [] @@ -90,18 +115,23 @@ def apply_with_depth(self, fn, depth=0): return ret def merge(self, rhs): - """Merge data of another tree. + """ + Merge data of another tree. + If self does not have some keys which rhs has, then new nodes are added. Parameters ---------- rhs: another TreeNode + """ def _merge(lhs, rhs): """ - lhs: self sub node - rhs: rhs sub node + Merge data. + + lhs: TreeNode which is updated + rhs: TreeNode whici is const """ lhs.data.extend(rhs.data) diff --git a/src/tilde_vis/tilde_vis/latency_viewer.py b/src/tilde_vis/tilde_vis/latency_viewer.py index f6f7a34a170..5f12d337833 100644 --- a/src/tilde_vis/tilde_vis/latency_viewer.py +++ b/src/tilde_vis/tilde_vis/latency_viewer.py @@ -13,80 +13,86 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Latency viewer node and its main function.""" + import argparse import curses import os import pickle -import sys from statistics import mean +import sys import time +from builtin_interfaces.msg import Time as TimeMsg import rclpy from rclpy.node import Node +from rclpy.qos import ( + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy + ) from rclpy.time import Time -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy - -from builtin_interfaces.msg import Time as TimeMsg from tilde_msg.msg import ( PubInfo, ) -from tilde_vis.pubinfo_traverse import TopicGraph, InputSensorStampSolver -from tilde_vis.pub_info import ( - PubInfo as PubInfoObj, - PubInfos as PubInfosObj - ) from tilde_vis.printer import ( - Printer, - NcursesPrinter + NcursesPrinter, + Printer ) from tilde_vis.pub_info import ( + PubInfo as PubInfoObj, + PubInfos as PubInfosObj, time2str ) +from tilde_vis.pubinfo_traverse import ( + InputSensorStampSolver, + TopicGraph + ) + EXCLUDES_TOPICS = [ - "/diagnostics/info/pub", - "/control/trajectory_follower/mpc_follower/debug/markers/info/pub", - "/control/trajectory_follower/mpc_follower/debug/steering_cmd/info/pub", - "/localization/debug/ellipse_marker/info/pub", - "/localization/pose_twist_fusion_filter/debug/info/pub", - "/localization/pose_twist_fusion_filter/debug/measured_pose/info/pub", - "/localization/pose_twist_fusion_filter/debug/stop_flag/info/pub", - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/area_with_objects/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/clearance_map/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/object_clearance_map/info/pub", # noqa: #501 - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/smoothed_points/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw/info/pub", # noqa: #501 - "/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled/info/pub", # noqa: #501 + '/diagnostics/info/pub', + '/control/trajectory_follower/mpc_follower/debug/markers/info/pub', + '/control/trajectory_follower/mpc_follower/debug/steering_cmd/info/pub', + '/localization/debug/ellipse_marker/info/pub', + '/localization/pose_twist_fusion_filter/debug/info/pub', + '/localization/pose_twist_fusion_filter/debug/measured_pose/info/pub', + '/localization/pose_twist_fusion_filter/debug/stop_flag/info/pub', + '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/area_with_objects/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/clearance_map/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/object_clearance_map/info/pub', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/smoothed_points/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw/info/pub', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled/info/pub', # noqa: #501 ] LEAVES = [ - "/initialpose", - "/map/pointcloud_map", - "/sensing/lidar/top/rectified/pointcloud", - "/sensing/imu/imu_data", - "/vehicle/status/twist", + '/initialpose', + '/map/pointcloud_map', + '/sensing/lidar/top/rectified/pointcloud', + '/sensing/imu/imu_data', + '/vehicle/status/twist', ] -PUB_INFO = "topic_infos.pkl" +PUB_INFO = 'topic_infos.pkl' TIMER_SEC = 1.0 -TARGET_TOPIC = "/sensing/lidar/concatenated/pointcloud" +TARGET_TOPIC = '/sensing/lidar/concatenated/pointcloud' STOPS = [ - "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias", # noqa: #501 + '/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias', # noqa: #501 ] -DUMP_DIR = "dump.d" +DUMP_DIR = 'dump.d' def truncate(s, prelen=20, n=80): - """Truncate string. + """ + Truncate string. Parameters ---------- @@ -97,6 +103,7 @@ def truncate(s, prelen=20, n=80): Return ------ truncated string such that "abc...edf" + """ assert prelen + 5 < n @@ -106,11 +113,14 @@ def truncate(s, prelen=20, n=80): pre = s[:prelen] post = s[len(s) - n + prelen + 5:] - return pre + "..." + post + return pre + '...' + post class LatencyStat(object): + """Latency statistics.""" + def __init__(self): + """Constructor.""" self.dur_ms_list = [] self.dur_pub_ms_list = [] self.dur_pub_ms_steady_list = [] @@ -118,9 +128,12 @@ def __init__(self): def add(self, r): """ + Add single result. + Parameters ---------- r: pubinfo_traverse.SolverResults + """ self.dur_ms_list.append(r.dur_ms) self.dur_pub_ms_list.append(r.dur_pub_ms) @@ -128,6 +141,7 @@ def add(self, r): self.is_leaf_list.append(r.is_leaf) def report(self): + """Report statistics.""" dur_ms_list = self.dur_ms_list is_leaf_list = self.is_leaf_list dur_pub_ms_list = self.dur_pub_ms_list @@ -148,69 +162,85 @@ def report(self): is_all_leaf = all(is_leaf_list) return { - "dur_min": dur_min, - "dur_mean": dur_mean, - "dur_max": dur_max, - "dur_pub_min": dur_pub_min, - "dur_pub_mean": dur_pub_mean, - "dur_pub_max": dur_pub_max, - "dur_pub_steady_min": dur_pub_steady_min, - "dur_pub_steady_mean": dur_pub_steady_mean, - "dur_pub_steady_max": dur_pub_steady_max, - "is_all_leaf": is_all_leaf, + 'dur_min': dur_min, + 'dur_mean': dur_mean, + 'dur_max': dur_max, + 'dur_pub_min': dur_pub_min, + 'dur_pub_mean': dur_pub_mean, + 'dur_pub_max': dur_pub_max, + 'dur_pub_steady_min': dur_pub_steady_min, + 'dur_pub_steady_mean': dur_pub_steady_mean, + 'dur_pub_steady_max': dur_pub_steady_max, + 'is_all_leaf': is_all_leaf, } class PerTopicLatencyStat(object): + """Per topic latency statistics.""" + def __init__(self): + """Constructor.""" self.data = {} def add(self, r): """ + Add single result. + Parameters ---------- r: pubinfo_traverse.SolverResults + """ self.data.setdefault(r.topic, LatencyStat()).add(r) def report(self): + """Get report as dictionary.""" ret = {} for (topic, stat) in self.data.items(): ret[topic] = stat.report() return ret def print_report(self, printer): + """ + Print report. + + Parameters + ---------- + printer: see printer.py + + """ logs = [] reports = self.report() - logs.append("{:80} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}".format( - "topic", "dur", "dur", "dur", "e2e", "e2e", "e2e", "e2e_s", "e2e_s", "e2e_s" + logs.append('{:80} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}'.format( + 'topic', 'dur', 'dur', 'dur', 'e2e', 'e2e', 'e2e', 'e2e_s', 'e2e_s', 'e2e_s' )) def p(v): if v > 1000: - return " inf" + return ' inf' else: - return "{:>6.1f}".format(v) + return '{:>6.1f}'.format(v) for (topic, report) in reports.items(): - s = f"{topic:80} " - s += f"{p(report['dur_min'])} " - s += f"{p(report['dur_mean'])} " - s += f"{p(report['dur_max'])} " - s += f"{p(report['dur_pub_min'])} " - s += f"{p(report['dur_pub_mean'])} " - s += f"{p(report['dur_pub_max'])} " - s += f"{p(report['dur_pub_steady_min'])} " - s += f"{p(report['dur_pub_steady_mean'])} " - s += f"{p(report['dur_pub_steady_max'])} " - s += f"{report['is_all_leaf']}" + s = f'{topic:80} ' + s += f'{p(report["dur_min"])} ' + s += f'{p(report["dur_mean"])} ' + s += f'{p(report["dur_max"])} ' + s += f'{p(report["dur_pub_min"])} ' + s += f'{p(report["dur_pub_mean"])} ' + s += f'{p(report["dur_pub_maxn"])} ' + s += f'{p(report["dur_pub_steady_min"])} ' + s += f'{p(report["dur_pub_steady_mean"])} ' + s += f'{p(report["dur_pub_steady_max"])} ' + s += f'{report["is_all_leaf"]}' logs.append(s) - printer.print(logs) + printer.print_(logs) def calc_one_hot(results): - """Calcurate one hot result. + """ + Calcurate one hot result. Paramters --------- @@ -253,7 +283,8 @@ def calc(node, depth): def handle_stat(stamps, pubinfos, target_topic, solver, stops, dumps=False): - """Handle stat core + """ + Calculate latency statistics. Parameters ---------- @@ -268,6 +299,7 @@ def handle_stat(stamps, pubinfos, target_topic, solver, stops, dumps=False): ------ TreeNode. .data: see calc_stat + """ idx = -3 if len(stamps) == 0: @@ -276,7 +308,7 @@ def handle_stat(stamps, pubinfos, target_topic, solver, stops, dumps=False): idx = 1 merged = None - print(f"idx: {idx}") + print(f'idx: {idx}') for target_stamp in stamps[:idx]: results = solver.solve2( pubinfos, target_topic, target_stamp, @@ -284,7 +316,8 @@ def handle_stat(stamps, pubinfos, target_topic, solver, stops, dumps=False): if dumps: pickle.dump(results, - open(f"{DUMP_DIR}/stat_results_{target_stamp}.pkl", "wb"), + open(f'{DUMP_DIR}/stat_results_{target_stamp}.pkl', + 'wb'), protocol=pickle.HIGHEST_PROTOCOL) results = update_stat(results) @@ -298,7 +331,8 @@ def handle_stat(stamps, pubinfos, target_topic, solver, stops, dumps=False): def update_stat(results): - """Update results to have durations + """ + Update results to have durations. Parameters ---------- @@ -309,6 +343,7 @@ def update_stat(results): results with results.data: [(dur_ms, dur_ms_steady)] results.data_orig = results.data + """ last_pub_time = Time.from_msg( results.data[0].out_info.pubsub_stamp) @@ -340,7 +375,8 @@ def _update_stat(node): def calc_stat(results): - """Calcurate statistics + """ + Calcurate statistics. Parameters ---------- @@ -358,6 +394,7 @@ def calc_stat(results): "dur_mean_steady" "dur_max_steady" "is_leaf" + """ def _calc_stat(node, depth): durs = [] @@ -375,40 +412,44 @@ def _calc(arr, fn): return fn(arr) return { - "depth": depth, - "name": node.name, - "dur_min": _calc(durs, min), - "dur_mean": _calc(durs, mean), - "dur_max": _calc(durs, max), - "dur_min_steady": _calc(durs_steady, min), - "dur_mean_steady": _calc(durs_steady, mean), - "dur_max_steady": _calc(durs_steady, max), - "is_leaf": is_leaf + 'depth': depth, + 'name': node.name, + 'dur_min': _calc(durs, min), + 'dur_mean': _calc(durs, mean), + 'dur_max': _calc(durs, max), + 'dur_min_steady': _calc(durs_steady, min), + 'dur_mean_steady': _calc(durs_steady, mean), + 'dur_max_steady': _calc(durs_steady, max), + 'is_leaf': is_leaf } return results.apply_with_depth(_calc_stat) class LatencyViewerNode(Node): + """Latency viewer node.""" + def __init__(self, stdscr=None): - """Constructor + """ + Constructor. Parameters ---------- stdscr: if not None, use ncurses + """ super().__init__('latency_viewer_node') - self.declare_parameter("excludes_topics", EXCLUDES_TOPICS) - self.declare_parameter("leaves", LEAVES) - self.declare_parameter("graph_pkl", "") - self.declare_parameter("timer_sec", TIMER_SEC) - self.declare_parameter("target_topic", TARGET_TOPIC) - self.declare_parameter("keep_info_sec", 3) - self.declare_parameter("wait_sec_to_init_graph", 10) - self.declare_parameter("mode", "stat") - self.declare_parameter("stops", STOPS) + self.declare_parameter('excludes_topics', EXCLUDES_TOPICS) + self.declare_parameter('leaves', LEAVES) + self.declare_parameter('graph_pkl', '') + self.declare_parameter('timer_sec', TIMER_SEC) + self.declare_parameter('target_topic', TARGET_TOPIC) + self.declare_parameter('keep_info_sec', 3) + self.declare_parameter('wait_sec_to_init_graph', 10) + self.declare_parameter('mode', 'stat') + self.declare_parameter('stops', STOPS) # whether to dump solver.solve() result - self.declare_parameter("dumps", False) + self.declare_parameter('dumps', False) print(stdscr) if stdscr is not None: @@ -421,7 +462,7 @@ def __init__(self, stdscr=None): self.topic_seq = {} excludes_topic = ( - self.get_parameter("excludes_topics") + self.get_parameter('excludes_topics') .get_parameter_value().string_array_value) topics = self.get_pub_info_topics(excludes=excludes_topic) logs = [] @@ -442,65 +483,67 @@ def __init__(self, stdscr=None): self.solver = None self.stops = ( - self.get_parameter("stops") + self.get_parameter('stops') .get_parameter_value().string_array_value) graph_pkl = ( - self.get_parameter("graph_pkl") + self.get_parameter('graph_pkl') .get_parameter_value().string_value) if graph_pkl: - graph = pickle.load(open(graph_pkl, "rb")) + graph = pickle.load(open(graph_pkl, 'rb')) self.solver = InputSensorStampSolver(graph) self.pub_infos = PubInfosObj() timer_sec = ( - self.get_parameter("timer_sec") + self.get_parameter('timer_sec') .get_parameter_value().double_value) self.timer = self.create_timer(timer_sec, self.timer_callback) self.target_topic = ( - self.get_parameter("target_topic") + self.get_parameter('target_topic') .get_parameter_value().string_value) self.keep_info_sec = ( - self.get_parameter("keep_info_sec") + self.get_parameter('keep_info_sec') .get_parameter_value().integer_value) self.wait_sec_to_init_graph = ( - self.get_parameter("wait_sec_to_init_graph"). + self.get_parameter('wait_sec_to_init_graph'). get_parameter_value().integer_value) self.wait_init = 0 self.init_skips() - self.printer.print(logs) + self.printer.print_(logs) self.dumps = ( - self.get_parameter("dumps") + self.get_parameter('dumps') .get_parameter_value().bool_value) if self.dumps: os.makedirs(DUMP_DIR, exist_ok=True) def init_skips(self): """ - Definition of skips. + Define skips. + See TopicGraph.__init__ comment. """ skips = {} - RECT_OUT_EX = "/sensing/lidar/{}/rectified/pointcloud_ex" - RECT_OUT = "/sensing/lidar/{}/rectified/pointcloud" - RECT_IN = "/sensing/lidar/{}/mirror_cropped/pointcloud_ex" + RECT_OUT_EX = '/sensing/lidar/{}/rectified/pointcloud_ex' + RECT_OUT = '/sensing/lidar/{}/rectified/pointcloud' + RECT_IN = '/sensing/lidar/{}/mirror_cropped/pointcloud_ex' # top - for pos in ["top", "left", "right"]: + for pos in ['top', 'left', 'right']: skips[RECT_OUT_EX.format(pos)] = RECT_IN.format(pos) skips[RECT_OUT.format(pos)] = RECT_IN.format(pos) - skips["/sensing/lidar/no_ground/pointcloud"] = \ - "/sensing/lidar/concatenated/pointcloud" + skips['/sensing/lidar/no_ground/pointcloud'] = \ + '/sensing/lidar/concatenated/pointcloud' self.skips = skips def listener_callback(self, pub_info_msg): + """Handle PubInfo message.""" st = time.time() output_info = pub_info_msg.output_info @@ -513,22 +556,22 @@ def listener_callback(self, pub_info_msg): seq = self.topic_seq[topic] if this_seq < seq: # skew - s = f"skew topic={topic} " + \ - f"msg_seq={this_seq}({time2str(output_info.header_stamp)})" + \ - f" saved_seq={seq}" + s = f'skew topic={topic} ' + \ + f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ + f' saved_seq={seq}' stamps = self.pub_infos.stamps(topic) if stamps or len(stamps) > 0: last_stamp = sorted(stamps)[-1] - s += f"({last_stamp})" + s += f'({last_stamp})' self.get_logger().info(s) elif seq + 1 < this_seq: # message drop happens - s = f"may drop topic={topic} " + \ - f"msg_seq={this_seq}({time2str(output_info.header_stamp)})" + \ - f" saved_seq={seq}" + s = f'may drop topic={topic} ' + \ + f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ + f' saved_seq={seq}' stamps = self.pub_infos.stamps(topic) if stamps or len(stamps) > 0: last_stamp = sorted(stamps)[-1] - s += f"({last_stamp})" + s += f'({last_stamp})' self.get_logger().info(s) self.topic_seq[topic] = this_seq else: @@ -553,10 +596,11 @@ def listener_callback(self, pub_info_msg): elasped_ms = (et - st) * 1000 if elasped_ms > 1: self.get_logger().info( - f"sub {topic} at {stamp}@ {elasped_ms} [ms]") + f'sub {topic} at {stamp}@ {elasped_ms} [ms]') def handle_stat(self, stamps): - """Report statistics + """ + Report statistics. Paramters --------- @@ -565,6 +609,7 @@ def handle_stat(self, stamps): Returns ------- list of string + """ pubinfos = self.pub_infos target_topic = self.target_topic @@ -581,45 +626,46 @@ def handle_stat(self, stamps): logs = [] - fmt = "{:80} " + \ - "{:>6} {:>6} {:>6} " + \ - "{:>6} {:>6} {:>6}" + fmt = '{:80} ' + \ + '{:>6} {:>6} {:>6} ' + \ + '{:>6} {:>6} {:>6}' logs.append(fmt.format( - "topic", - "e2e", "e2e", "e2e", - "e2e_s", "e2e_s", "e2e_s" + 'topic', + 'e2e', 'e2e', 'e2e', + 'e2e_s', 'e2e_s', 'e2e_s' )) for stat in stats: - name = (" " * stat["depth"] + - stat["name"] + - ("*" if stat["is_leaf"] else "")) + name = (' ' * stat['depth'] + + stat['name'] + + ('*' if stat['is_leaf'] else '')) name = truncate(name) def p(v): if v is None: - s = "NA" - return f"{s:>6}" + s = 'NA' + return f'{s:>6}' if v > 1000: - s = "inf" - return f"{s:>6}" + s = 'inf' + return f'{s:>6}' else: - return f"{v:>6.1f}" + return f'{v:>6.1f}' - s = f"{name:80} " - s += f"{p(stat['dur_min']):>6} " - s += f"{p(stat['dur_mean']):>6} " - s += f"{p(stat['dur_max']):>6} " - s += f"{p(stat['dur_min_steady']):>6} " - s += f"{p(stat['dur_mean_steady']):>6} " - s += f"{p(stat['dur_max_steady']):>6} " + s = f'{name:80} ' + s += f'{p(stat["dur_min"]):>6} ' + s += f'{p(stat["dur_mean"]):>6} ' + s += f'{p(stat["dur_max"]):>6} ' + s += f'{p(stat["dur_min_steady"]):>6} ' + s += f'{p(stat["dur_mean_steady"]):>6} ' + s += f'{p(stat["dur_max_steady"]):>6} ' logs.append(s) return logs def handle_one_hot(self, stamps): - """Report only one message + """ + Report only one message. Paramters --------- @@ -628,6 +674,7 @@ def handle_one_hot(self, stamps): Returns ------- array of strings for log + """ pubinfos = self.pub_infos target_topic = self.target_topic @@ -648,29 +695,30 @@ def handle_one_hot(self, stamps): if self.dumps: pickle.dump(results, - open(f"{DUMP_DIR}/onehot_results_{target_stamp}.pkl", - "wb"), + open(f'{DUMP_DIR}/onehot_results_{target_stamp}.pkl', + 'wb'), protocol=pickle.HIGHEST_PROTOCOL) onehot_durs = calc_one_hot(results) logs = [] for e in onehot_durs: (depth, name, dur_ms, dur_ms_steady, is_leaf, stamp) = e - name = truncate(" " * depth + name + ("*" if is_leaf else "")) + name = truncate(' ' * depth + name + ('*' if is_leaf else '')) if dur_ms is None: - dur_ms = "NA" + dur_ms = 'NA' if dur_ms_steady is None: - dur_ms_steady = "NA" - stamp_s = "NA" + dur_ms_steady = 'NA' + stamp_s = 'NA' if stamp: stamp_s = time2str(stamp) - s = f"{name:80} {stamp_s:>20} {dur_ms:>6} {dur_ms_steady:>6}" + s = f'{name:80} {stamp_s:>20} {dur_ms:>6} {dur_ms_steady:>6}' logs.append(s) return logs def timer_callback(self): + """Clear old data.""" st = time.time() pubinfos = self.pub_infos target_topic = self.target_topic @@ -678,53 +726,54 @@ def timer_callback(self): stamps = sorted(pubinfos.stamps(target_topic)) logs = [] str_stamp = stamps[0] if len(stamps) > 0 else '' - logs.append(f"stamps: {len(stamps)}, {str_stamp}") + logs.append(f'stamps: {len(stamps)}, {str_stamp}') if len(stamps) == 0: - self.printer.print(logs) + self.printer.print_(logs) return # check header.stamp field existence if not pubinfos.get(target_topic, stamps[0]).out_info.has_stamp: - logs.append("**WARNING** target topic has no stamp field") - self.printer.print(logs) + logs.append('**WARNING** target topic has no stamp field') + self.printer.print_(logs) return if not self.solver: if self.wait_init < self.wait_sec_to_init_graph: - self.printer.print(logs) + self.printer.print_(logs) self.wait_init += 1 return - logs.append("init solver") + logs.append('init solver') graph = TopicGraph(pubinfos, skips=self.skips) self.solver = InputSensorStampSolver(graph) - mode = self.get_parameter("mode").get_parameter_value().string_value - if mode == "stat": + mode = self.get_parameter('mode').get_parameter_value().string_value + if mode == 'stat': logs.extend(self.handle_stat(stamps)) - elif mode == "onehot": + elif mode == 'onehot': logs.extend(self.handle_one_hot(stamps)) else: - logs.append("unknown mode") + logs.append('unknown mode') et1 = time.time() handle_ms = (et1 - st) * 1000 # cleanup PubInfos - (latest_sec, latest_ns) = map(lambda x: int(x), stamps[-1].split(".")) + (latest_sec, latest_ns) = map(lambda x: int(x), stamps[-1].split('.')) until_stamp = TimeMsg(sec=latest_sec - self.keep_info_sec, nanosec=latest_ns) pubinfos.erase_until(until_stamp) et2 = time.time() cleanup_ms = (et2 - et1) * 1000 - self.printer.print(logs) + self.printer.print_(logs) if handle_ms + cleanup_ms > 30: - s = f"timer handle_ms={handle_ms}" + \ - f" cleanup_ms={cleanup_ms}" + s = f'timer handle_ms={handle_ms}' + \ + f' cleanup_ms={cleanup_ms}' self.get_logger().info(s) def get_pub_info_topics(self, excludes=[]): - """Get all topic infos + """ + Get all topic infos. Paramters --------- @@ -733,6 +782,7 @@ def get_pub_info_topics(self, excludes=[]): ------- map a list of pubinfo topics + """ # short sleep between node creation and get_topic_names_and_types # https://github.com/ros2/ros2/issues/1057 @@ -740,7 +790,7 @@ def get_pub_info_topics(self, excludes=[]): # but don't need in dev environment(i.e. rosbagged PubInfo) time.sleep(0.5) - msg_type = "tilde_msg/msg/PubInfo" + msg_type = 'tilde_msg/msg/PubInfo' topic_and_types = self.get_topic_names_and_types() filtered_topic_and_types = \ filter(lambda x: msg_type in x[1], topic_and_types) @@ -751,6 +801,7 @@ def get_pub_info_topics(self, excludes=[]): def main_curses(stdscr, args=None): + """Wrap main function for ncurses.""" rclpy.init(args=args) node = LatencyViewerNode(stdscr=stdscr) @@ -759,21 +810,23 @@ def main_curses(stdscr, args=None): def main(args=None): + """Main.""" parser = argparse.ArgumentParser() parser.add_argument( - "--batch", action="store_true", - help="Run as batch mode, just like top command `-b` option") + '--batch', action='store_true', + help='Run as batch mode, just like top command `-b` option') parsed = parser.parse_known_args()[0] print(parsed) if not parsed.batch: - print("not batch") + print('not batch') curses.wrapper(main_curses, args=sys.argv) else: - print("batch") + print('batch') main_curses(None, args=sys.argv) if __name__ == '__main__': + """Main.""" main(sys.argv) diff --git a/src/tilde_vis/tilde_vis/parse_pub_info.py b/src/tilde_vis/tilde_vis/parse_pub_info.py index a86e40b3559..11811c949d6 100755 --- a/src/tilde_vis/tilde_vis/parse_pub_info.py +++ b/src/tilde_vis/tilde_vis/parse_pub_info.py @@ -13,16 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pickle +"""Convert rosbag of PubInfo to pkl file.""" + import argparse +import pickle + +from rclpy.serialization import deserialize_message import rosbag2_py from rosidl_runtime_py.utilities import get_message -from rclpy.serialization import deserialize_message from tilde_vis.pub_info import PubInfo, PubInfos def get_rosbag_options(path, serialization_format='cdr'): + """Get rosbag options.""" storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') converter_options = rosbag2_py.ConverterOptions( @@ -33,6 +37,7 @@ def get_rosbag_options(path, serialization_format='cdr'): def run(args): + """Do main loop.""" bag_path = args.bag_path storage_options, converter_options = get_rosbag_options(bag_path) @@ -57,7 +62,7 @@ def run(args): (topic, data, t) = reader.read_next() # TODO: need more accurate check - if "/info/pub" not in topic: + if '/info/pub' not in topic: continue msg_type = get_message(type_map[topic]) @@ -70,23 +75,25 @@ def run(args): print(cnt) cnt += 1 - pickle.dump(out_per_topic, open("topic_infos.pkl", "wb"), + pickle.dump(out_per_topic, open('topic_infos.pkl', 'wb'), protocol=pickle.HIGHEST_PROTOCOL) for topic, count in skip_topic_vs_count.items(): - print(f"skipped {topic} {count} times") + print(f'skipped {topic} {count} times') def main(): + """Main.""" parser = argparse.ArgumentParser() - parser.add_argument("bag_path") + parser.add_argument('bag_path') parser.add_argument( - "--cnt", type=int, default=-1, - help="number of messages to dump (whole */info/pub, not per topic)") + '--cnt', type=int, default=-1, + help='number of messages to dump (whole */info/pub, not per topic)') args = parser.parse_args() run(args) -if __name__ == "__main__": +if __name__ == '__main__': + """Main.""" main() diff --git a/src/tilde_vis/tilde_vis/pathnode_vis.py b/src/tilde_vis/tilde_vis/pathnode_vis.py index 76327b05c44..aa07d5647c7 100644 --- a/src/tilde_vis/tilde_vis/pathnode_vis.py +++ b/src/tilde_vis/tilde_vis/pathnode_vis.py @@ -13,11 +13,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""LatencyViewer PoC version. Deprecated.""" -import os from collections import defaultdict -import numpy as np from logging import basicConfig, getLogger +import os + +import numpy as np import rclpy from rclpy.node import Node @@ -54,29 +56,36 @@ class TopicInfoStatistics(object): + """TopicInfo statistics.""" + def __init__(self, topics, max_rows=10): + """Constructor.""" self.topics = topics self.t2i = {topic: i for i, topic in enumerate(topics)} - self.seq2time = defaultdict(lambda: - np.ones(len(self.t2i), dtype=np.float64)) + self.seq2time = defaultdict( + lambda: - np.ones(len(self.t2i), dtype=np.float64)) # (n_messages, n_subcallbacks), nanoseconds self.data = - np.ones((max_rows, len(topics)), dtype=np.float) self.data_idx = 0 self.max_rows = max_rows self.num_dump = -1 - s = "" + s = '' for t in topics: - s += f"{t} " + s += f'{t} ' s = s.rstrip() - s = s.replace(" ", " -> ") + s = s.replace(' ', ' -> ') print(s) - def set(self, seq, topic, callback_start_time): + def register(self, seq, topic, callback_start_time): + """Register result.""" vec = self.seq2time[seq] i = self.t2i[topic] vec[i] = callback_start_time - logger.debug("seq {}: i: {} non zero: {}".format(seq, i, np.count_nonzero(vec > 0))) - if np.count_nonzero(vec > 0) == len(self.t2i) and self.data_idx < self.max_rows: + logger.debug('seq {}: i: {} non zero: {}'.format( + seq, i, np.count_nonzero(vec > 0))) + if (np.count_nonzero(vec > 0) == len(self.t2i) and + self.data_idx < self.max_rows): self.data[self.data_idx, ...] = vec[...] del self.seq2time[seq] self.data_idx += 1 @@ -84,12 +93,14 @@ def set(self, seq, topic, callback_start_time): # TODO: delete too old seq key (now leaks) def is_filled(self): + """Check data.""" return self.data_idx == self.max_rows def dump_and_clear(self, dumps=False): + """Dump and clear results.""" if dumps: self.num_dump += 1 - fname = "{}.npy".format(self.num_dump) + fname = '{}.npy'.format(self.num_dump) np.save(fname, self.data) # TODO: ignore nan(-1) field @@ -109,41 +120,50 @@ def dump_and_clear(self, dumps=False): min_e2e = e2e.min() def fmt(vec): - s = "" + s = '' for v in vec: - s += f"{v:5.1f} " + s += f'{v:5.1f} ' return s.rstrip() - print("max: " + fmt(maxtime)) - print("avg: " + fmt(avgtime)) - print("min: " + fmt(mintime)) - print("e2e: " + fmt([max_e2e, avg_e2e, min_e2e])) - print("") + print('max: ' + fmt(maxtime)) + print('avg: ' + fmt(avgtime)) + print('min: ' + fmt(mintime)) + print('e2e: ' + fmt([max_e2e, avg_e2e, min_e2e])) + print('') self.data[...] = -1.0 self.data_idx = 0 class PathVisNode(Node): + """Path visualization node.""" + def __init__(self): + """Constructor.""" super().__init__('path_vis_node') - self.declare_parameter("topics", LIDAR_PREPROCESS) - self.declare_parameter("window", 10) - self.declare_parameter("dump", False) - self.declare_parameter("watches_pub", False) - - topics = self.get_parameter("topics").get_parameter_value().string_array_value - window = self.get_parameter("window").get_parameter_value().integer_value - watches_pub = self.get_parameter("watches_pub").get_parameter_value().bool_value - info_name = "/info/sub" + self.declare_parameter('topics', LIDAR_PREPROCESS) + self.declare_parameter('window', 10) + self.declare_parameter('dump', False) + self.declare_parameter('watches_pub', False) + + topics = self.get_parameter('topics') \ + .get_parameter_value() \ + .string_array_value + window = self.get_parameter('window') \ + .get_parameter_value() \ + .integer_value + watches_pub = self.get_parameter('watches_pub') \ + .get_parameter_value() \ + .bool_value + info_name = '/info/sub' if watches_pub: - info_name = "/info/pub" + info_name = '/info/pub' topics = LIDAR_PREPROCESS_PUB self.statistics = TopicInfoStatistics(topics, window) self.subs = [] - logger.debug("info_name: {}".format(info_name)) + logger.debug('info_name: {}'.format(info_name)) for topic in topics: sub = self.create_subscription( @@ -155,18 +175,20 @@ def __init__(self): self.subs.append(sub) def listener_callback(self, topic_info): + """Store data and print results.""" seq = topic_info.seq topic = topic_info.topic_name start_time = Time.from_msg(topic_info.callback_start).nanoseconds - dump = self.get_parameter("dump").get_parameter_value().bool_value + dump = self.get_parameter('dump').get_parameter_value().bool_value - logger.debug("{} {}".format(seq, topic)) - self.statistics.set(seq, topic, start_time) + logger.debug('{} {}'.format(seq, topic)) + self.statistics.register(seq, topic, start_time) if self.statistics.is_filled(): self.statistics.dump_and_clear(dump) def main(args=None): + """Main.""" rclpy.init(args=args) node = PathVisNode() @@ -176,4 +198,5 @@ def main(args=None): if __name__ == '__main__': + """Main.""" main() diff --git a/src/tilde_vis/tilde_vis/printer.py b/src/tilde_vis/tilde_vis/printer.py index a516ccefe7e..7012b6559e9 100644 --- a/src/tilde_vis/tilde_vis/printer.py +++ b/src/tilde_vis/tilde_vis/printer.py @@ -12,18 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Printer for stdout w/wo interactions.""" + import curses import datetime import re class Printer(object): - def print(self, lines): + """Print messages to stdout.""" + + def print_(self, lines): + """Print logs.""" for s in lines: print(s) class NcursesPrinter(object): + """Print messages with interaction.""" + MODE_STOP = 0 MODE_RUN = 1 MODE_REGEXP = 2 @@ -34,6 +41,7 @@ class NcursesPrinter(object): NPAGE = curses.KEY_NPAGE def __init__(self, stdscr): + """Constructor.""" stdscr.scrollok(True) stdscr.timeout(0) # set non-blocking self.stdscr = stdscr @@ -55,7 +63,7 @@ def __init__(self, stdscr): self.start_line = 0 # regexp mode - self.regexp_ptn = "" + self.regexp_ptn = '' self.search = None self.adjust_keys = ( @@ -84,6 +92,7 @@ def __init__(self, stdscr): ) def adjust_lines(self, k, lines): + """Move lines by interaction.""" step = 0 if k in (self.DOWN, ord('j')): step = 1 @@ -107,9 +116,11 @@ def adjust_lines(self, k, lines): self.start_line = len(lines) - 1 def enter_regex_mode(self): + """Change mode.""" self.mode = self.MODE_REGEXP - def print(self, input_lines): + def print_(self, input_lines): + """Print logs.""" stdscr = self.stdscr lines = self.lines @@ -161,7 +172,7 @@ def print(self, input_lines): elif k in (curses.KEY_ENTER, ord('\n')): self.mode = self.MODE_RUN else: - raise Exception("unknown mode") + raise Exception('unknown mode') stdscr.clear() @@ -170,34 +181,35 @@ def print(self, input_lines): if i + 1 == self.y_max - 1: break - if s[-1] != "\n": - s += "\n" + if s[-1] != '\n': + s += '\n' lineno = self.start_line + i - stdscr.addstr(i+1, 0, f"{lineno:<3}| ") + stdscr.addstr(i+1, 0, f'{lineno:<3}| ') stdscr.addstr(i+1, 5, s) stdscr.refresh() def print_mode(self): + """Print mode line.""" stdscr = self.stdscr color = self.CYAN t = datetime.datetime.now().strftime('%Y/%m/%d %H:%M:%S') - mode_str = "" + mode_str = '' if self.mode == self.MODE_RUN: mode_str = \ - "press UP/DOWN/PgUp/PgDown/jkdugG to scroll," + \ - "r or '/' to filter" + 'press UP/DOWN/PgUp/PgDown/jkdugG to scroll,' + \ + 'r or "/" to filter' color = self.WHITE elif self.mode == self.MODE_STOP: - mode_str = "scrolling... Press q/F to periodically update" + mode_str = 'scrolling... Press q/F to periodically update' color = self.CYAN elif self.mode == self.MODE_REGEXP: - mode_str = "Regexp: " + self.regexp_ptn + "_" + mode_str = 'Regexp: ' + self.regexp_ptn + '_' color = self.CYAN s = mode_str - s += " " * (self.x_max - len(mode_str) - len(t)) + s += ' ' * (self.x_max - len(mode_str) - len(t)) s += t stdscr.addstr(0, 0, s, color) diff --git a/src/tilde_vis/tilde_vis/pub_info.py b/src/tilde_vis/tilde_vis/pub_info.py index b9492d94753..5ebb214e588 100644 --- a/src/tilde_vis/tilde_vis/pub_info.py +++ b/src/tilde_vis/tilde_vis/pub_info.py @@ -12,20 +12,26 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Internal data structures for PubInfo.""" + from rclpy.time import Time def time2str(t): - """ - t: builtin_interfaces.msg.Time - """ - return f"{t.sec}.{t.nanosec:09d}" + """Convert builtin_interfaces.msg.Time to string.""" + return f'{t.sec}.{t.nanosec:09d}' class TopicInfo(object): + """Represent output_info and input_infos of PubInfo message.""" + def __init__(self, topic, pubsub_stamp, pubsub_stamp_steady, has_stamp, stamp): """ + Hold information similar to PubInfo in/out info. + + Parameters + ---------- topic: target topic [string] pubsub_stamp: when publish or subscription callback is called @@ -35,6 +41,7 @@ def __init__(self, topic, pubsub_stamp, pubsub_stamp_steady, [builtin_interfaces.mg.Time] has_stamp: whether main topic has header.stamp or not [bool] stamp: header.stamp [builtin_interfaces.msg.Time] + """ self.topic = topic self.pubsub_stamp = pubsub_stamp @@ -43,23 +50,31 @@ def __init__(self, topic, pubsub_stamp, pubsub_stamp_steady, self.stamp = stamp def __str__(self): - stamp_s = time2str(self.stamp) if self.has_stamp else "NA" - return f"TopicInfo(topic={self.topic}, stamp={stamp_s})" + """Get string.""" + stamp_s = time2str(self.stamp) if self.has_stamp else 'NA' + return f'TopicInfo(topic={self.topic}, stamp={stamp_s})' class PubInfo(object): """ - PubInfo - - out_info = TopicInfo - - in_infos = {topic_name => list of TopicInfo} + Hold information similar to PubInfoMsg. + + TODO(y-okumura-isp): Can we use PubInfoMsg directly? + """ + def __init__(self, out_topic, pub_time, pub_time_steady, has_stamp, out_stamp): """ + Constructor. + + Parameters + ---------- out_topic: topic name pub_time: when publish main topic [builtin_interfaces.msg.Time] has_stamp: whether main topic has header.stamp [bool] out_stamp: main topic header.stamp [builtin_interfaces.msg.Time] + """ self.out_info = TopicInfo(out_topic, pub_time, pub_time_steady, has_stamp, out_stamp) @@ -69,29 +84,38 @@ def __init__(self, out_topic, pub_time, pub_time_steady, def add_input_info(self, in_topic, sub_stamp, sub_stamp_steady, has_stamp, stamp): """ + Add input info. + + Parameters + ---------- in_topic: topic string has_stamp: bool stamp: header stamp [builtin_interfaces.msg.Time] + """ info = TopicInfo(in_topic, sub_stamp, sub_stamp_steady, has_stamp, stamp) self.in_infos.setdefault(in_topic, []).append(info) def __str__(self): - s = "PubInfo: \n" - s += f" out_info={self.out_info}\n" + """Get string.""" + s = 'PubInfo: \n' + s += f' out_info={self.out_info}\n' for _, infos in self.in_infos.items(): for info in infos: - s += f" in_infos={info}\n" + s += f' in_infos={info}\n' return s @property def out_topic(self): + """Get output topic.""" return self.out_info.topic @staticmethod def fromMsg(pub_info_msg): """ + Convert PubInfoMsg to PubInfo. + Parameters ---------- pub_info_msg: PubInfoMsg @@ -99,6 +123,7 @@ def fromMsg(pub_info_msg): Returns ------- PubInfo + """ output_info = pub_info_msg.output_info pub_info = PubInfo(output_info.topic_name, @@ -116,17 +141,21 @@ def fromMsg(pub_info_msg): class PubInfos(object): - ''' - topic vs PubInfo + """ + Hold topic vs PubInfo. We have double-key dictionary internally, i.e. we can get PubInfo by topic_vs_pubinfo[topic_name][stamp]. - ''' + + """ + def __init__(self): + """Constructor.""" # {topic => {stamp_str => PubInfo}} self.topic_vs_pubinfos = {} def add(self, pubinfo): + """Add a PubInfo.""" out_topic = pubinfo.out_info.topic out_stamp = time2str(pubinfo.out_info.stamp) if out_topic not in self.topic_vs_pubinfos.keys(): @@ -140,18 +169,27 @@ def add(self, pubinfo): def erase_until(self, stamp): """ - erase added pubinfo where out_stamp < stamp + Erase added pubinfo where out_stamp < stamp. + + Parameters + ---------- stamp: builtin_interfaces.msg.Time + """ def time_ge(lhs, rhs): - """helper function to compare stamps i.e. self.topic_vs_pubinfos[*].keys(). + """ + Compare time. + + Helper function to compare stamps i.e. + self.topic_vs_pubinfos[*].keys(). As stamps are string, it is not appropriate to compare as string. Builtin_msg.msg.Time does not implement `<=>`, we use rclpy.Time, althoght clock_type has no meaning. + """ - [lhs_sec, lhs_nsec] = map(lambda x: int(x), lhs.split(".")) - [rhs_sec, rhs_nsec] = map(lambda x: int(x), rhs.split(".")) + [lhs_sec, lhs_nsec] = map(lambda x: int(x), lhs.split('.')) + [rhs_sec, rhs_nsec] = map(lambda x: int(x), rhs.split('.')) lhs_time = Time(seconds=lhs_sec, nanoseconds=lhs_nsec) rhs_time = Time(seconds=rhs_sec, nanoseconds=rhs_nsec) return lhs_time <= rhs_time @@ -169,12 +207,11 @@ def time_ge(lhs, rhs): del self.topic_vs_pubinfos[topic][stamp] def topics(self): + """Get topic names which has registered PubInfo.""" return list(self.topic_vs_pubinfos.keys()) def stamps(self, topic): - """ - return List[stamps] - """ + """Get List[stamps].""" if topic not in self.topic_vs_pubinfos.keys(): return [] @@ -182,10 +219,17 @@ def stamps(self, topic): def get(self, topic, stamp=None, idx=None): """ + Get a PubInfo. + + Parameters + ---------- topic: str stamp: str such as 1618559286.884563157 - return PubInfo or None + Return + ------ + PubInfo or None + """ ret = None if topic not in self.topic_vs_pubinfos.keys(): @@ -201,7 +245,7 @@ def get(self, topic, stamp=None, idx=None): if idx is not None: if len(infos.keys()) <= idx: - print("args.idx too large, should be < {len(info.kens())}") + print('args.idx too large, should be < {len(info.kens())}') else: key = sorted(infos.keys())[idx] ret = infos[key] @@ -210,8 +254,12 @@ def get(self, topic, stamp=None, idx=None): def in_topics(self, topic): """ - gather input topics by ignoring stamps - return set of topic name + Gather input topics by ignoring stamps. + + Return + ------ + set of topic name + """ ret = set() @@ -224,6 +272,7 @@ def in_topics(self, topic): return ret def all_topics(self): + """Get all topics which are used as input or output.""" out = set() lhs = self.topics() diff --git a/src/tilde_vis/tilde_vis/pubinfo_traverse.py b/src/tilde_vis/tilde_vis/pubinfo_traverse.py index f09d9f00b4c..175d1206a44 100755 --- a/src/tilde_vis/tilde_vis/pubinfo_traverse.py +++ b/src/tilde_vis/tilde_vis/pubinfo_traverse.py @@ -13,28 +13,37 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pickle +"""PubInfo Traverser.""" + import argparse -from collections import deque, defaultdict -import time +from collections import defaultdict, deque import json +import pickle +import time from rclpy.time import Time -from tilde_vis.pub_info import time2str from tilde_vis.data_as_tree import TreeNode +from tilde_vis.pub_info import time2str def strstamp2time(strstamp): - sec, nanosec = strstamp.split(".") + """Convert string time to Time.""" + sec, nanosec = strstamp.split('.') return Time(seconds=int(sec), nanoseconds=int(nanosec)) class SolverResult(object): + """SolverResult.""" + def __init__(self, topic, stamp, dur_ms, dur_pub_ms, dur_pub_ms_steady, is_leaf, parent): """ + Constructor. + + Parameters + ---------- topic: topic [string] stamp: header.stamp [rclpy.Time] dur_ms: duration of header.stamp in ms [double] @@ -42,6 +51,7 @@ def __init__(self, topic, stamp, dur_ms, dur_pub_ms_steady: same with above but in steady clock ms [double] is_leaf: bool parent: parent topic [string] + """ self.topic = topic self.stamp = stamp @@ -53,9 +63,13 @@ def __init__(self, topic, stamp, dur_ms, class SolverResultsPrinter(object): + """SolverResultsPrinter.""" + @classmethod def as_tree(cls, results): - """ Construct array of string to print. + """ + Construct array of string to print. + This methos returns tree command like expression from output topic to source topics. Multiple input topics are shown by indented listing. @@ -75,16 +89,21 @@ def as_tree(cls, results): Return ------ array of string + """ pass class SolverResults(object): + """SolverResults.""" + def __init__(self): + """Constructor.""" self.data = [] # list of Result def add(self, *args): - """ Register Result. + """ + Register Result. Paramaters ---------- @@ -94,7 +113,10 @@ def add(self, *args): class InputSensorStampSolver(object): + """InputSensorStampSolver.""" + def __init__(self, graph): + """Constructor.""" # {topic: {stamp: {sensor_topic: [stamps]}}} self.topic_stamp_to_sensor_stamp = {} self.graph = graph @@ -104,14 +126,21 @@ def __init__(self, graph): def solve(self, pubinfos, tgt_topic, tgt_stamp, stops=[]): """ + Traverse from (tgt_topic, tgt_stamp)-message. + + Parameters + ---------- topic: target topic stamp: target stamp(str) stops: list of stop topics to prevent loop - return SolverResults + Return + ------ + SolverResults Calcurate topic_stamp_to_sensor_stamp internally. + """ graph = self.graph path_bfs = graph.bfs_rev(tgt_topic) @@ -133,7 +162,7 @@ def solve(self, pubinfos, tgt_topic, tgt_stamp, dists[tgt_topic][tgt_stamp] = 1 queue.append((tgt_topic, tgt_stamp, start_pub_time, start_pub_time_steady)) - parentQ.append("") + parentQ.append('') ret = SolverResults() while len(queue) != 0: @@ -182,7 +211,8 @@ def solve(self, pubinfos, tgt_topic, tgt_stamp, def solve2(self, pubinfos, tgt_topic, tgt_stamp, stops=[]): - """Traverse DAG from output to input. + """ + Traverse DAG from output to input. Parameters ---------- @@ -199,10 +229,11 @@ def solve2(self, pubinfos, tgt_topic, tgt_stamp, returned TreeNode preserve entire graph. - .name means topic - .data is PubInfo of the topic. [] whn PubInfo loss + """ skips = self.skips stamp = tgt_stamp - key = tgt_topic + ".".join(stops) + key = tgt_topic + '.'.join(stops) if key not in self.empty_results: self.empty_results[key] = self._solve_empty(tgt_topic, stops=stops) empty_results = self.empty_results[key] @@ -241,6 +272,7 @@ def solve2(self, pubinfos, tgt_topic, tgt_stamp, return root_results def append(self, topic, stamp, sensor_topic, sensor_stamp): + """Append results to topic_stamp_to_sensor_stamp.""" dic = self.topic_stamp_to_sensor_stamp if topic not in dic.keys(): dic[topic] = {} @@ -253,7 +285,9 @@ def append(self, topic, stamp, sensor_topic, sensor_stamp): def _solve_empty(self, tgt_topic, stops=[]): - """Get empty results to know graph + """ + Get empty results to know graph. + Parameters ---------- tgt_topic: output topic [string] @@ -262,6 +296,7 @@ def _solve_empty(self, tgt_topic, Returns ------- TreeNode + """ skips = self.skips graph = self.graph @@ -288,17 +323,19 @@ def _solve_empty(self, tgt_topic, class TopicGraph(object): - "Construct topic graph by ignoring stamps" + """Construct topic graph by ignoring stamps.""" def __init__(self, pubinfos, skips={}): """ - Parameters - ---------- def init_solver(): + Constructor. + Parameters + ---------- pubinfos: PubInfos skips: skip topics. {downstream: upstream} by input-to-output order ex) {"/sensing/lidar/top/rectified/pointcloud_ex": "/sensing/lidar/top/mirror_cropped/pointcloud_ex"} + """ self.topics = sorted(pubinfos.all_topics()) self.t2i = {t: i for i, t in enumerate(self.topics)} @@ -321,28 +358,44 @@ def __init__(self, pubinfos, skips={}): self.rev_edges[out_id].add(in_id) def dump(self, fname): + """Dump.""" out = {} - out["topics"] = self.topics - out["topic2id"] = self.t2i - out["topic_edges"] = [list(l) for l in self.topic_edges] - out["rev_edges"] = [list(l) for l in self.rev_edges] + out['topics'] = self.topics + out['topic2id'] = self.t2i + out['topic_edges'] = [list(l) for l in self.topic_edges] + out['rev_edges'] = [list(l) for l in self.rev_edges] - json.dump(out, open(fname, "wt")) + json.dump(out, open(fname, 'wt')) def rev_topics(self, topic): """ - get input topics - return List[Topic] + Get input topics of the target topic. + + Parameters + ---------- + topic: topic name + + Return + ------ + List[Topic] + """ out_topic_idx = self.t2i[topic] return [self.topics[i] for i in self.rev_edges[out_topic_idx]] def dfs_rev(self, start_topic): - ''' - traverse topic graph reversely from start_topic + """ + Do depth first search from the topic. - return topic names in appearance order - ''' + Parameters + ---------- + start_topic: topic name + + Return + ------ + topic names in appearance order + + """ edges = self.rev_edges n = len(edges) seen = [False for _ in range(n)] @@ -363,7 +416,16 @@ def dfs(v): def bfs_rev(self, start_topic): """ - return list of (topic, is_leaf) + Do breadth first search from the topic. + + Parameters + ---------- + start_topic: topic name + + Return + ------ + topic names in appearance order + """ edges = self.rev_edges n = len(edges) @@ -388,17 +450,18 @@ def bfs_rev(self, start_topic): def run(args): + """Run.""" pklfile = args.pickle_file - pubinfos = pickle.load(open(pklfile, "rb")) + pubinfos = pickle.load(open(pklfile, 'rb')) tgt_topic = args.topic tgt_stamp = sorted(pubinfos.stamps(tgt_topic))[args.stamp_index] graph = TopicGraph(pubinfos) - print("dump") - graph.dump("graph.json") - pickle.dump(graph, open("graph.pkl", "wb"), + print('dump') + graph.dump('graph.json') + pickle.dump(graph, open('graph.pkl', 'wb'), protocol=pickle.HIGHEST_PROTOCOL) # bfs_path = graph.bfs_rev(tgt_topic) @@ -412,22 +475,23 @@ def run(args): solver.solve(pubinfos, tgt_topic, tgt_stamp) et = time.time() - print(f"solve {(et-st) * 1000} [ms]") + print(f'solve {(et-st) * 1000} [ms]') def main(): + """Main.""" parser = argparse.ArgumentParser() - parser.add_argument("pickle_file") - parser.add_argument("stamp_index", type=int, default=0, - help="header stamp index") - parser.add_argument("topic", - default="/sensing/lidar/no_ground/pointcloud", - nargs="?") + parser.add_argument('pickle_file') + parser.add_argument('stamp_index', type=int, default=0, + help='header stamp index') + parser.add_argument('topic', + default='/sensing/lidar/no_ground/pointcloud', + nargs='?') args = parser.parse_args() run(args) -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/src/tilde_vis_test/README.md b/src/tilde_vis_test/README.md new file mode 100644 index 00000000000..6c64356ea58 --- /dev/null +++ b/src/tilde_vis_test/README.md @@ -0,0 +1,31 @@ +# tilde_vis_test + +## Description + +Supplementary files to test `tilde_vis` package. + +## issues_23_1.launch.py + +See #23. It's OK if the program runs with no error. + +```bash +$ ros2 launch tilde_vis_test issues_23_1.launch.py +[filter-1] [INFO] [1654561227.800326321] [filter]: Filter get message cnt_ = 0 +[filter-1] [INFO] [1654561228.300162503] [filter]: Filter get message cnt_ = 1 +[filter-1] [INFO] [1654561228.800174003] [filter]: Filter get message cnt_ = 2 +[filter-1] [INFO] [1654561229.300167408] [filter]: Filter get message cnt_ = 3 +[filter-1] [INFO] [1654561229.800184144] [filter]: Filter get message cnt_ = 4 +``` + +## issues_23_2.launch.py + +See #23. It's OK if the program runs with no error. +Be aware that FPS is very slow to reproduce bug. + +```bash +$ ros2 launch tilde_vis_test issues_23_2.launch.py + +[relay-1] [INFO] [1654561285.390170828] [relay]: Relay get message cnt_ = 0 +[relay-1] [INFO] [1654561290.390119277] [relay]: Relay get message cnt_ = 1 +[relay-1] [INFO] [1654561295.390336942] [relay]: Relay get message cnt_ = 2 +``` diff --git a/src/tilde_vis_test/launch/issues_23_1.launch.py b/src/tilde_vis_test/launch/issues_23_1.launch.py index b713f748f84..849180182b0 100644 --- a/src/tilde_vis_test/launch/issues_23_1.launch.py +++ b/src/tilde_vis_test/launch/issues_23_1.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): return LaunchDescription([ Node( - name="filter", + name='filter', package='tilde_vis_test', executable='filter', output='screen'), @@ -31,6 +31,6 @@ def generate_launch_description(): executable='sensor', output='screen', parameters=[{ - "timer_us": 500 * 1000, + 'timer_us': 500 * 1000, }]), ]) diff --git a/src/tilde_vis_test/launch/issues_23_2.launch.py b/src/tilde_vis_test/launch/issues_23_2.launch.py index 45a1cc87625..e78adbda43d 100644 --- a/src/tilde_vis_test/launch/issues_23_2.launch.py +++ b/src/tilde_vis_test/launch/issues_23_2.launch.py @@ -30,6 +30,6 @@ def generate_launch_description(): executable='sensor', output='screen', parameters=[{ - "timer_us": 5 * 1000 * 1000, + 'timer_us': 5 * 1000 * 1000, }]), ]) diff --git a/src/tilde_vis_test/src/tilde_vis_test.cpp b/src/tilde_vis_test/src/tilde_vis_test.cpp index aeab2c89270..0af5591d2d4 100644 --- a/src/tilde_vis_test/src/tilde_vis_test.cpp +++ b/src/tilde_vis_test/src/tilde_vis_test.cpp @@ -36,8 +36,8 @@ using namespace std::chrono_literals; namespace tilde_vis_test { -const std::string in_topic = "in"; -const std::string out_topic = "out"; +const char in_topic[] = "in"; +const char out_topic[] = "out"; class Sensor : public rclcpp::Node { @@ -79,19 +79,19 @@ class Relay : public tilde::TildeNode rclcpp::QoS qos(rclcpp::KeepLast(7)); pub_pc_ = this->create_tilde_publisher( - out_topic, qos); + out_topic, qos); sub_pc_ = this->create_tilde_subscription( - in_topic, qos, - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void - { - (void) msg; - RCLCPP_INFO( - this->get_logger(), - "Relay get message cnt_ = %d", - cnt_); - pub_pc_->publish(std::move(msg)); - cnt_++; + in_topic, qos, + [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void + { + (void) msg; + RCLCPP_INFO( + this->get_logger(), + "Relay get message cnt_ = %d", + cnt_); + pub_pc_->publish(std::move(msg)); + cnt_++; }); } @@ -110,21 +110,21 @@ class Filter : public tilde::TildeNode rclcpp::QoS qos(rclcpp::KeepLast(7)); pub_str_ = this->create_tilde_publisher( - out_topic, qos); + out_topic, qos); sub_pc_ = this->create_tilde_subscription( - in_topic, qos, - [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void - { - (void) msg; - RCLCPP_INFO( - this->get_logger(), - "Filter get message cnt_ = %d", - cnt_); - auto omsg = std::make_unique(); - omsg->data = "hello"; - pub_str_->publish(std::move(omsg)); - cnt_++; + in_topic, qos, + [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void + { + (void) msg; + RCLCPP_INFO( + this->get_logger(), + "Filter get message cnt_ = %d", + cnt_); + auto omsg = std::make_unique(); + omsg->data = "hello"; + pub_str_->publish(std::move(omsg)); + cnt_++; }); }