def test_registration_list(self): var1 = 0.0 registry = StatisticsRegistry(DEFAULT_TOPIC) registration_list = [] registry.registerFunction("var1", (lambda: var1), registration_list) self.evaluate_msgs({"var1": 0.0}, registry) del registration_list self.evaluate_msgs({}, registry)
def test_map_data(self): map_data = {"x": 20.0, "y": 124.2, "z": 20} registry = StatisticsRegistry(DEFAULT_TOPIC) for key in map_data: registry.registerFunction("var_{}".format(key), (lambda map_key=key: map_data[map_key])) self.evaluate_msgs({"var_x": 20.0, "var_y": 124.2, "var_z": 20}, registry) # Testing by modifying the data in the dictionary map_data["x"] = 25.7 map_data["z"] += 7 self.evaluate_msgs({"var_x": 25.7, "var_y": 124.2, "var_z": 27}, registry)
def test_list_data(self): list_data = [0, 10, 20, 30] registry = StatisticsRegistry(DEFAULT_TOPIC) for i in range(0, len(list_data)): registry.registerFunction("var_{}".format(i), (lambda index=i: list_data[index])) self.evaluate_msgs({"var_0": 0.0, "var_1": 10.0, "var_2": 20.0, "var_3": 30.0}, registry) # Testing by modifying the data in the list list_data[2] = 152.2 list_data[3] = 1.23 self.evaluate_msgs({"var_0": 0.0, "var_1": 10.0, "var_2": 152.2, "var_3": 1.23}, registry)
def test_basic(self): var1 = 0.0 registry = StatisticsRegistry(DEFAULT_TOPIC) registry.registerFunction("var1", (lambda: var1)) self.evaluate_msgs({"var1": 0.0}, registry) var1 = 1.0 msg = registry.createFullMsg() self.evaluate_msgs({"var1": 1.0}, registry) var2 = 2 registry.registerFunction("var2", (lambda: var2)) msg = registry.createFullMsg() self.evaluate_msgs({"var1": 1.0, "var2": 2.0}, registry) registry.unregister("var1") self.evaluate_msgs({"var2": 2.0}, registry)
def test_publish(self): sub = rospy.Subscriber(DEFAULT_TOPIC, Statistics, self.msg_cb) registry = StatisticsRegistry(DEFAULT_TOPIC) self.last_msg = None rospy.sleep(0.5) #Time for pub-sub connection registry.publish() self.wait_for_msg() self.evaluate_msgs({}, self.last_msg) var = 1.0 registry.registerFunction("var", (lambda: var)) self.last_msg = None registry.publish() self.wait_for_msg() self.evaluate_msgs({"var": 1.0}, self.last_msg) self.last_msg = None registry.publishCustomStatistic("foo", 23) self.wait_for_msg() self.evaluate_msgs({"foo": 23}, self.last_msg)
def test_publish(self): sub = rospy.Subscriber(DEFAULT_TOPIC + "/full", Statistics, self.full_msg_cb) names_sub = rospy.Subscriber(DEFAULT_TOPIC + "/names", StatisticsNames, self.names_msg_cb) values_sub = rospy.Subscriber(DEFAULT_TOPIC + "/values", StatisticsValues, self.values_msg_cb) registry = StatisticsRegistry(DEFAULT_TOPIC) rospy.sleep(0.5) #Time for pub-sub connection self.clear() registry.publish() self.wait_for_msg() self.compare_full_msg({}, self.last_full_msg) self.assertEqual(self.last_names_msg.names_version, self.last_values_msg.names_version) old_names_ver = self.last_names_msg.names_version var = 1.0 registry.registerFunction("var", (lambda: var)) self.clear() registry.publish() self.wait_for_msg() self.compare_full_msg({"var": 1.0}, self.last_full_msg) self.assertEqual(self.last_names_msg.names_version, self.last_values_msg.names_version) self.assertNotEqual(old_names_ver, self.last_names_msg.names_version) old_names_ver = self.last_names_msg.names_version #If we publish the same statistics, names_version shouldn't change self.clear() registry.publish() self.wait_for_msg() self.assertEqual(old_names_ver, self.last_names_msg.names_version) self.clear() registry.publishCustomStatistic("foo", 23) rospy.sleep(0.2) #hard coded sleep only for full msg self.compare_full_msg({"foo": 23}, self.last_full_msg)