def test_list_overwrite_entry(self, key, lists): first_values, second_values = lists db = GodMap() db.safe_set_data([key], first_values) for i, v in enumerate(first_values): self.assertEqual(db.safe_get_data([key, i]), v) db.safe_set_data([key, i], second_values[i]) for i, v in enumerate(second_values): self.assertEqual(db.safe_get_data([key, i]), v)
def test_class1(self, key, class_name, key_values): c = type(str(class_name), (object, ), {})() for k, v in zip(*key_values): setattr(c, k, None) db = GodMap() db.safe_set_data([key], c) for k, v in zip(*key_values): self.assertEqual(db.safe_get_data([key, k]), None) for k, v in zip(*key_values): db.safe_set_data([key, k], v) for k, v in zip(*key_values): self.assertEqual(db.safe_get_data([key, k]), v)
def test_function3(self, key1, key2, key3, key4, key5): db = GodMap() class MUH(object): def __call__(self, next_member): return [key5] a = MUH() d = {key2: a} db.safe_set_data([key1], d) try: db.safe_get_data([key1, key2, (key3, key4), 0]) assert False except TypeError: assert True
def test_dict2(self, key, key_values): d = {} db = GodMap() db.safe_set_data([key], d) for k, v in zip(*key_values): db.safe_set_data([key, k], v) for k, v in zip(*key_values): self.assertEqual(db.safe_get_data([key, k]), v)
def test_list_double_index(self): key = 'asdf' value = np.array([[0, 1], [2, 3]]) db = GodMap() db.safe_set_data([key], value) for i in range(value.shape[0]): for j in range(value.shape[1]): self.assertEqual(db.safe_get_data([key, i, j]), value[i, j])
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0), False) gm.safe_set_data([u'world'], w) assert r == gm.safe_get_data([u'world', u'robot'])
def test_class3(self, class_names): db = GodMap() for i, name in enumerate(class_names): c = type(str(name), (object, ), {})() db.safe_set_data(class_names[:i + 1], c) for i, name in enumerate(class_names): c = db.safe_get_data(class_names[:i + 1]) self.assertEqual(type(c).__name__, name)
def test_function_2param_call(self, key1, key2, key3, key4): db = GodMap() class MUH(object): def __call__(self, next_member, next_next_member): return next_next_member a = MUH() d = {key2: a} db.safe_set_data([key1], d) self.assertEqual(db.safe_get_data([key1, key2, (key3, key4)]), key4)
def test_function4(self, key1, key2, key3, key4, key5): db = GodMap() class MUH(object): def __call__(self, next_member, next_next_member): return [key5] a = MUH() d = {key2: a} db.safe_set_data([key1], d) self.assertEqual(key5, db.safe_get_data([key1, key2, (key3, key4), 0]))
def test_function_no_param(self, key1, key2, key3): db = GodMap() class MUH(object): def __call__(self): return [key3] a = MUH() d = {key2: a} db.safe_set_data([key1], d) self.assertEqual(key3, db.safe_get_data([key1, key2, [], 0]))
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(robot=r, base_pose=PoseStamped(), controlled_joints=[], joint_vel_limit=KeyDefaultDict(lambda key: 0), joint_acc_limit=KeyDefaultDict(lambda key: 0), joint_weights=KeyDefaultDict(lambda key: 0), calc_self_collision_matrix=False, ignored_pairs=set(), added_pairs=set(), symengine_backend='llvm', symengine_opt_level=0) gm.safe_set_data([u'world'], w) gm_robot = gm.safe_get_data(identifier.robot) assert 'pr2' == gm_robot.get_name()
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.safe_get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.safe_set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.safe_get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.safe_set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.safe_get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(e) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weights, identifier.default_joint_weight_identifier, god_map) process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map) joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map) joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map) world = PyBulletWorld(god_map.safe_get_data(identifier.gui), blackboard.god_map.safe_get_data(identifier.data_folder)) god_map.safe_set_data(identifier.world, world) robot = WorldObject(god_map.safe_get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True, ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions), added_pairs=god_map.safe_get_data(identifier.added_self_collisions), symengine_backend=god_map.safe_get_data(identifier.symengine_backend), symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map) return god_map
def test_tuple1(self, key, value): value = tuple(value) db = GodMap() db.safe_set_data([key], value) for i, v in enumerate(value): self.assertEqual(db.safe_get_data([key, i]), v)
def test_list_negative_index(self, key, l): db = GodMap() db.safe_set_data([key], l) for i in range(len(l)): self.assertEqual(db.safe_get_data([key, -i]), l[-i])
def test_function_1param_lambda(self, key, key2): db = GodMap() f = lambda x: x db.safe_set_data([key], f) self.assertEqual(db.safe_get_data([key, [key2]]), key2)
def test_namedtuple(self, key, tuple_name, key_values): Frame = namedtuple(tuple_name, key_values[0]) db = GodMap() db.safe_set_data([key], Frame(*key_values[1])) for k, v in zip(*key_values): self.assertEqual(db.safe_get_data([key, k]), v)
def test_set_get_float(self, key, number): db = GodMap() db.safe_set_data([key], number) db.safe_set_data([key], number) self.assertEqual(db.safe_get_data([key]), number)
def test_set_get_integer(self, key, number): db = GodMap() db.safe_set_data([key], number) self.assertEqual(db.safe_get_data([key]), number, msg=u'key={}, number={}'.format(key, number))
def test_dict3(self, key, tuple_key, value): tuple_key = tuple(tuple_key) d = {tuple_key: value} db = GodMap() db.safe_set_data([key], d) self.assertEqual(db.safe_get_data([key, tuple_key]), value)