def test_add_object(self): ks = ArticulationModel() obj = bb(some_str='lol', some_scalar=7.5, some_subobj=bb(x=4, y=5, z=10)) op = CreateComplexObject(Path('my_obj'), obj) self.assertIn('path/some_str', op.mod_paths) self.assertIn('path/some_scalar', op.mod_paths) self.assertIn('path/some_subobj', op.mod_paths) self.assertIn('path/some_subobj/x', op.mod_paths) self.assertIn('path/some_subobj/y', op.mod_paths) self.assertIn('path/some_subobj/z', op.mod_paths) self.assertIn('path', op._root_set) self.assertEquals(op.mod_paths['path/some_str'], ('my_obj','some_str')) self.assertEquals(op.mod_paths['path/some_scalar'], ('my_obj','some_scalar')) self.assertEquals(op.mod_paths['path/some_subobj'], ('my_obj','some_subobj')) self.assertEquals(op.mod_paths['path/some_subobj/x'],('my_obj','some_subobj','x')) self.assertEquals(op.mod_paths['path/some_subobj/y'],('my_obj','some_subobj','y')) self.assertEquals(op.mod_paths['path/some_subobj/z'],('my_obj','some_subobj','z')) self.assertEquals(op._root_set['path'], ('my_obj')) ks.apply_operation('create my_obj', op) self.assertTrue(ks.has_data('my_obj')) self.assertEquals(ks.get_data('my_obj/some_str'), 'lol') self.assertEquals(ks.get_data('my_obj/some_scalar'), 7.5) self.assertEquals(ks.get_data('my_obj/some_subobj/x'), 4) self.assertEquals(ks.get_data('my_obj/some_subobj/y'), 5) self.assertEquals(ks.get_data('my_obj/some_subobj/z'), 10) ks.remove_operation('create my_obj') self.assertFalse(ks.has_data('my_obj')) self.assertFalse(ks.has_data('my_obj/some_scalar')) self.assertFalse(ks.has_data('my_obj/some_subobj')) self.assertFalse(ks.has_data('my_obj/some_subobj/x')) self.assertFalse(ks.has_data('my_obj/some_subobj/y')) self.assertFalse(ks.has_data('my_obj/some_subobj/z'))
def fake_op(deps, mods): return bb(args_paths={n: Path(p) for n, p in enumerate(deps)}, mod_paths={n: Path(p) for n, p in enumerate(mods)})
def cb_srv_get_constraints(csymbs): return server.srv_get_constraints(bb(symbols=csymbs))
def cb_srv_get_model(paths, csymbs): return server.srv_get_model( bb(paths=paths, constrained_symbols=csymbs))
def test_apply_operation(self): self.ops_msg = None self.model_msg = None def cb_update(ops, model): self.ops_msg = ops self.model_msg = model server = ModelServerForTesting(cb_update) def cb_srv_get_model(paths, csymbs): return server.srv_get_model( bb(paths=paths, constrained_symbols=csymbs)) def cb_srv_get_constraints(csymbs): return server.srv_get_constraints(bb(symbols=csymbs)) op_client = OperationsClientForTesting(None, True) m_client = ModelClientForTesting(cb_srv_get_model, cb_srv_get_constraints, None) m_client.has_data('testbot') urdf_model = URDF.from_xml_file( res_pkg_path('package://kineverse/urdf/testbot.urdf')) load_urdf(op_client, urdf_model.name, urdf_model) op_list = op_client.apply_changes() req = bb(operations=op_list) server.srv_apply_operations(req) server.srv_apply_operations(req) op_client.cb_operations_update(self.ops_msg) with open('update.json', 'w') as f: json.dump(dict( zip(self.model_msg.update.paths, self.model_msg.update.data)), f, indent=2) m_client.cb_model_update(self.model_msg) o_model = server.km.get_data('testbot') t_model = m_client.get_data('testbot') with open('original.json', 'w') as f: json.dump(o_model, f, indent=True) with open('transmitted.json', 'w') as f: json.dump(t_model, f, indent=True) self.assertEquals(o_model.name, t_model.name) l = o_model.links['arm_mount_link'] lt = t_model.links['arm_mount_link'] for n, l in o_model.links.items(): if n not in t_model.links: print('Link {} missing from transmitted model'.format(n)) continue lt = t_model.links[n] if l == lt: print('Equality check passed for link {}'.format(n)) print('Types: {} {}'.format(type(l), type(lt))) print('parent: {}'.format(lt.parent == l.parent)) print('pose: {}'.format(lt.pose == l.pose)) print('to_parent: {}'.format(lt.to_parent == l.to_parent)) print('geometry: {}'.format(lt.geometry == l.geometry)) print('collision: {}'.format(lt.collision == l.collision)) print('inertial: {}\n---'.format(lt.inertial == l.inertial)) else: print('Equality check failed for link {}'.format('arm_mount_link')) print('Types: {} {}'.format(type(l), type(lt))) print('parent: {}'.format(lt.parent == l.parent)) print('pose: {}\n{}'.format(lt.pose == l.pose, l.pose - lt.pose)) print('to_parent: {}\n{}'.format(lt.to_parent == l.to_parent, l.to_parent - lt.to_parent)) print('geometry: {}'.format(lt.geometry == l.geometry)) print('collision: {}'.format(lt.collision == l.collision)) print('inertial: {}'.format(lt.inertial == l.inertial)) self.assertEquals(o_model.joints, t_model.joints) self.assertEquals(o_model, t_model)
def test_modifcation_events(self): km = EventModel() self.change_a = None self.change_b = None self.change_c = None self.change_c_f = None self.change_c_g = None def cb_change_a(data): self.change_a = data def cb_change_b(data): self.change_b = data def cb_change_c(data): self.change_c = data def cb_change_c_f(data): self.change_c_f = data def cb_change_c_g(data): self.change_c_g = data km.register_on_model_changed(Path('a'), cb_change_a) km.register_on_model_changed(Path('b'), cb_change_b) km.register_on_model_changed(Path('c'), cb_change_c) km.register_on_model_changed(Path('c/f'), cb_change_c_f) km.register_on_model_changed(Path('c/g'), cb_change_c_g) km.set_data('a', 5) km.dispatch_events() self.assertEquals(self.change_a, 5) self.assertEquals(self.change_b, None) km.set_data('b', 6) km.dispatch_events() self.assertEquals(self.change_b, 6) self.assertEquals(self.change_c, None) self.change_b = None km.set_data('b', 6) km.dispatch_events() self.assertEquals(self.change_b, None) c_1 = bb(f='lol', g=False) c_2 = bb(f='kek', g=False) c_3 = bb(f='foo', g=True) km.set_data('c', c_1) km.dispatch_events() self.assertEquals(self.change_c, c_1) self.assertEquals(self.change_c_f, 'lol') self.assertEquals(self.change_c_g, False) self.change_c = None self.change_c_g = None km.set_data('c/f', 'kek') km.dispatch_events() self.assertEquals(self.change_c, c_1) self.assertEquals(self.change_c_f, 'kek') self.assertEquals(self.change_c_g, None) km.set_data('c', c_3) km.dispatch_events() self.assertEquals(self.change_c, c_3) self.assertEquals(self.change_c_f, 'foo') self.assertEquals(self.change_c_g, True) km.remove_data('c') km.dispatch_events() self.assertEquals(self.change_c, None) self.assertEquals(self.change_c_f, None) self.assertEquals(self.change_c_g, None) self.change_b = None km.deregister_on_model_changed(cb_change_b) km.set_data('b', 42) km.dispatch_events() self.assertEquals(self.change_b, None)