def lock() -> Lock: """Creates lock with initialized scene and project.""" test = "test" lock = Lock({}) scene = UpdateableCachedScene(cmn.Scene(test, description=test)) lock.scene = scene project = UpdateableCachedProject(cmn.Project(test, lock.scene.id, description=test, has_logic=True)) lock.project = project assert lock.scene == scene assert lock.scene_or_exception() == scene assert lock.project == project assert lock.project_or_exception() == project # add some scene and project objects test_object = cmn.SceneObject(test, "TestType") lock.scene.upsert_object(test_object) ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap", cmn.Position(0, 0, 0)) ap_ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap", cmn.Position(0, 0, 1), ap.id) ap_ap_ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap_ap", cmn.Position(0, 0, 2), ap_ap.id) lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap2", cmn.Position(0, 1, 0)) ori = cmn.NamedOrientation("ori", cmn.Orientation()) lock.project.upsert_orientation(ap_ap_ap.id, ori) action = cmn.Action("action", "test/type", parameters=[], flows=[]) lock.project.upsert_action(ap_ap_ap.id, action) return lock
async def test_ctx_read_lock() -> None: test = "test" user = "******" glob.LOCK = Lock({}) assert await glob.LOCK.get_locked_roots_count() == 0 glob.LOCK.scene = UpdateableCachedScene(cmn.Scene(test, description=test)) glob.LOCK.project = UpdateableCachedProject(cmn.Project(test, glob.LOCK.scene.id, description=test, has_logic=True)) async def patch() -> set[str]: return {glob.LOCK.project_or_exception().id, glob.LOCK.scene_or_exception().id} storage.get_project_ids = storage.get_scene_ids = patch # add some scene and project objects test_object = cmn.SceneObject(test, "TestType") glob.LOCK.scene.upsert_object(test_object) ap = glob.LOCK.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap", cmn.Position(0, 0, 0), test_object.id) ap_ap = glob.LOCK.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap", cmn.Position(0, 0, 1), ap.id) assert await glob.LOCK.get_locked_roots_count() == 0 await glob.LOCK.write_lock(ap_ap.id, user, True) assert await glob.LOCK.is_write_locked(test_object.id, user) assert await glob.LOCK.is_write_locked(ap.id, user) assert await glob.LOCK.is_write_locked(ap_ap.id, user) async with ctx_read_lock(test_object.id, user): pass assert await glob.LOCK.is_write_locked(test_object.id, user) assert await glob.LOCK.is_write_locked(ap.id, user) assert await glob.LOCK.is_write_locked(ap_ap.id, user)
async def update_object_pose_using_robot_cb( req: srpc.o.UpdateObjectPoseUsingRobot.Request, ui: WsClient) -> None: """Updates object's pose using a pose of the robot's end effector. :param req: :return: """ assert glob.SCENE ensure_scene_started() if req.args.id == req.args.robot.robot_id: raise Arcor2Exception("Robot cannot update its own pose.") scene_object = glob.SCENE.object(req.args.id) obj_type = glob.OBJECT_TYPES[scene_object.type] if not obj_type.meta.has_pose: raise Arcor2Exception("Object without pose.") object_model = obj_type.meta.object_model if object_model: collision_model = object_model.model() if isinstance(collision_model, object_type.Mesh ) and req.args.pivot != req.args.PivotEnum.MIDDLE: raise Arcor2Exception( "Only middle pivot point is supported for objects with mesh collision model." ) elif req.args.pivot != req.args.PivotEnum.MIDDLE: raise Arcor2Exception( "Only middle pivot point is supported for objects without collision model." ) new_pose = await get_end_effector_pose(req.args.robot.robot_id, req.args.robot.end_effector) position_delta = common.Position() if object_model: collision_model = object_model.model() if isinstance(collision_model, object_type.Box): if req.args.pivot == req.args.PivotEnum.TOP: position_delta.z -= collision_model.size_z / 2 elif req.args.pivot == req.args.PivotEnum.BOTTOM: position_delta.z += collision_model.size_z / 2 elif isinstance(collision_model, object_type.Cylinder): if req.args.pivot == req.args.PivotEnum.TOP: position_delta.z -= collision_model.height / 2 elif req.args.pivot == req.args.PivotEnum.BOTTOM: position_delta.z += collision_model.height / 2 elif isinstance(collision_model, object_type.Sphere): if req.args.pivot == req.args.PivotEnum.TOP: position_delta.z -= collision_model.radius / 2 elif req.args.pivot == req.args.PivotEnum.BOTTOM: position_delta.z += collision_model.radius / 2 position_delta = position_delta.rotated(new_pose.orientation) assert scene_object.pose scene_object.pose.position = new_pose.position - position_delta scene_object.pose.orientation.set_from_quaternion( new_pose.orientation.as_quaternion() * quaternion.quaternion(0, 1, 0, 0)) asyncio.ensure_future(update_scene_object_pose(scene_object)) return None
def test_run_simple_project(start_processes: None, ars: ARServer) -> None: event(ars, events.c.ShowMainScreen) assert ars.call_rpc( rpc.s.NewScene.Request(get_id(), rpc.s.NewScene.Request.Args("Test scene")), rpc.s.NewScene.Response).result scene_data = event(ars, events.s.OpenScene).data assert scene_data scene = scene_data.scene event(ars, events.s.SceneState) assert ars.call_rpc( rpc.s.AddObjectToScene.Request( get_id(), rpc.s.AddObjectToScene.Request.Args("time_actions", TimeActions.__name__)), rpc.s.AddObjectToScene.Response, ).result obj = event(ars, events.s.SceneObjectChanged).data assert obj assert ars.call_rpc( rpc.s.AddObjectToScene.Request( get_id(), rpc.s.AddObjectToScene.Request.Args("random_actions", RandomActions.__name__)), rpc.s.AddObjectToScene.Response, ).result obj2 = event(ars, events.s.SceneObjectChanged).data assert obj2 # ------------------------------------------------------------------------------------------------------------------ assert ars.call_rpc( rpc.p.NewProject.Request( get_id(), rpc.p.NewProject.Request.Args(scene.id, "Project name")), rpc.p.NewProject.Response, ).result event(ars, events.s.SceneSaved) proj = event(ars, events.p.OpenProject).data assert proj event(ars, events.s.SceneState) assert ars.call_rpc( rpc.p.AddActionPoint.Request( get_id(), rpc.p.AddActionPoint.Request.Args("ap1", common.Position())), rpc.p.AddActionPoint.Response, ).result ap = event(ars, events.p.ActionPointChanged).data assert ap is not None assert ars.call_rpc( rpc.p.AddProjectParameter.Request( get_id(), rpc.p.AddProjectParameter.Request.Args("min_time", "double", json.dumps(0.45))), rpc.p.AddActionPoint.Response, ).result c1 = event(ars, events.p.ProjectParameterChanged).data assert c1 assert ars.call_rpc( rpc.p.AddAction.Request( get_id(), rpc.p.AddAction.Request.Args( ap.id, "test_action", f"{obj2.id}/{RandomActions.random_double.__name__}", [ common.ActionParameter( "range_min", common.ActionParameter.TypeEnum.PROJECT_PARAMETER, json.dumps(c1.id)), common.ActionParameter("range_max", "double", "0.55"), ], [common.Flow(outputs=["random_value"])], ), ), rpc.p.AddAction.Response, ).result action = event(ars, events.p.ActionChanged).data assert action is not None assert ars.call_rpc( rpc.p.AddAction.Request( get_id(), rpc.p.AddAction.Request.Args( ap.id, "test_action2", f"{obj.id}/{TimeActions.sleep.__name__}", [ common.ActionParameter( "seconds", common.ActionParameter.TypeEnum.LINK, json.dumps( f"{action.id}/{common.FlowTypes.DEFAULT}/0"), ) ], [common.Flow()], ), ), rpc.p.AddAction.Response, ).result action2 = event(ars, events.p.ActionChanged).data assert action2 is not None add_logic_item(ars, common.LogicItem.START, action.id) event(ars, events.lk.ObjectsUnlocked) add_logic_item(ars, action.id, action2.id) event(ars, events.lk.ObjectsUnlocked) add_logic_item(ars, action2.id, common.LogicItem.END) event(ars, events.lk.ObjectsUnlocked) save_project(ars) LOGGER.debug(project_service.get_project(proj.project.id)) # TODO test also temporary package close_project(ars) # ------------------------------------------------------------------------------------------------------------------ event(ars, events.c.ShowMainScreen) assert ars.call_rpc( rpc.b.BuildProject.Request( get_id(), rpc.b.BuildProject.Request.Args(proj.project.id, "Package name")), rpc.b.BuildProject.Response, ).result package = event(ars, eevents.PackageChanged).data assert package is not None assert ars.call_rpc( erpc.RunPackage.Request(get_id(), erpc.RunPackage.Request.Args(package.id)), erpc.RunPackage.Response).result ps = event(ars, arcor2_events.PackageState).data assert ps assert ps.package_id == package.id assert ps.state == ps.state.RUNNING pi = event(ars, arcor2_events.PackageInfo).data assert pi assert pi.package_id == package.id # random_double action act_state_before = event(ars, arcor2_events.ActionStateBefore).data assert act_state_before assert act_state_before.action_id == action.id assert act_state_before.parameters assert len(act_state_before.parameters) == 2 act_state_after = event(ars, arcor2_events.ActionStateAfter).data assert act_state_after assert act_state_after.action_id == action.id assert act_state_after.results assert len(act_state_after.results) == 1 assert isinstance(json.loads(act_state_after.results[0]), float) # sleep action act2_state_before = event(ars, arcor2_events.ActionStateBefore).data assert act2_state_before assert act2_state_before.action_id == action2.id assert act2_state_before.parameters assert len(act2_state_before.parameters) == 1 act2_state_after = event(ars, arcor2_events.ActionStateAfter).data assert act2_state_after assert act2_state_after.action_id == action2.id assert act2_state_after.results is None # TODO pause, resume assert ars.call_rpc(erpc.StopPackage.Request(get_id()), erpc.StopPackage.Response).result ps2 = wait_for_event(ars, arcor2_events.PackageState).data assert ps2 assert ps2.package_id == package.id assert ps2.state == ps.state.STOPPING ps3 = wait_for_event(ars, arcor2_events.PackageState).data assert ps3 assert ps3.package_id == package.id assert ps3.state == ps.state.STOPPED show_main_screen_event = event(ars, events.c.ShowMainScreen) assert show_main_screen_event.data assert show_main_screen_event.data.what == events.c.ShowMainScreen.Data.WhatEnum.PackagesList assert show_main_screen_event.data.highlight == package.id
async def update_object_pose_using_robot_cb(req: rpc.objects.UpdateObjectPoseUsingRobotRequest, ui: WsClient) -> \ None: """ Updates object's pose using a pose of the robot's end effector. :param req: :return: """ assert glob.SCENE if req.args.id == req.args.robot.robot_id: raise Arcor2Exception("Robot cannot update its own pose.") scene_object = glob.SCENE.object(req.args.id) if glob.OBJECT_TYPES[scene_object.type].needs_services: raise Arcor2Exception("Can't manipulate object created by service.") obj_inst = glob.SCENE_OBJECT_INSTANCES[req.args.id] if obj_inst.collision_model: if isinstance(obj_inst.collision_model, object_type.Mesh ) and req.args.pivot != rpc.objects.PivotEnum.MIDDLE: raise Arcor2Exception( "Only middle pivot point is supported for objects with mesh collision model." ) elif req.args.pivot != rpc.objects.PivotEnum.MIDDLE: raise Arcor2Exception( "Only middle pivot point is supported for objects without collision model." ) new_pose = await get_end_effector_pose(req.args.robot.robot_id, req.args.robot.end_effector) position_delta = common.Position() if obj_inst.collision_model: if isinstance(obj_inst.collision_model, object_type.Box): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.size_z / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.size_z / 2 elif isinstance(obj_inst.collision_model, object_type.Cylinder): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.height / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.height / 2 elif isinstance(obj_inst.collision_model, object_type.Sphere): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.radius / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.radius / 2 position_delta = position_delta.rotated(new_pose.orientation) scene_object.pose.position.x = new_pose.position.x - position_delta.x scene_object.pose.position.y = new_pose.position.y - position_delta.y scene_object.pose.position.z = new_pose.position.z - position_delta.z scene_object.pose.orientation.set_from_quaternion( new_pose.orientation.as_quaternion() * quaternion.quaternion(0, 1, 0, 0)) obj_inst.pose = scene_object.pose glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=scene_object))) OBJECTS_WITH_UPDATED_POSE.add(scene_object.id) return None
def project(ars: ARServer, scene: common.Scene) -> common.Project: """Creates project with following objects: ap - global AP ap_ap - child of ap ap_ap_ap - child of ap_ap ori - ap_ap_ap orientation """ test = "Test project" assert ars.call_rpc( rpc.p.NewProject.Request( uid(), rpc.p.NewProject.Request.Args(scene.id, test, test)), rpc.p.NewProject.Response).result project_evt = event(ars, events.p.OpenProject) assert project_evt.data event(ars, events.s.SceneState) assert ars.call_rpc( rpc.p.AddActionPoint.Request( uid(), rpc.p.AddActionPoint.Request.Args("ap", common.Position(0, 0, 0))), rpc.p.AddActionPoint.Response, ).result ap_evt = event(ars, events.p.ActionPointChanged) assert ap_evt.data assert ars.call_rpc( rpc.p.AddActionPoint.Request( uid(), rpc.p.AddActionPoint.Request.Args("ap_ap", common.Position(0, 0, 1), ap_evt.data.id)), rpc.p.AddActionPoint.Response, ).result ap_ap_evt = event(ars, events.p.ActionPointChanged) assert ap_ap_evt.data assert ars.call_rpc( rpc.p.AddActionPoint.Request( uid(), rpc.p.AddActionPoint.Request.Args("ap_ap_ap", common.Position(0, 0, 2), ap_ap_evt.data.id)), rpc.p.AddActionPoint.Response, ).result ap_ap_ap_evt = event(ars, events.p.ActionPointChanged) assert ap_ap_ap_evt.data lock_object(ars, ap_ap_ap_evt.data.id) assert ars.call_rpc( rpc.p.AddActionPointOrientation.Request( uid(), rpc.p.AddActionPointOrientation.Request.Args( ap_ap_ap_evt.data.id, common.Orientation(), "ori")), rpc.p.AddActionPointOrientation.Response, ).result ori_evt = event(ars, events.p.OrientationChanged) assert ori_evt.data unlock_object(ars, ap_ap_ap_evt.data.id) assert ars.call_rpc(rpc.p.SaveProject.Request(uid()), rpc.p.SaveProject.Response).result event(ars, events.p.ProjectSaved) assert ars.call_rpc(rpc.p.CloseProject.Request(uid()), rpc.p.CloseProject.Response).result event(ars, events.p.ProjectClosed) event(ars, events.c.ShowMainScreen) return project_evt.data.project
def test_project_const(start_processes: None, ars: ARServer) -> None: event(ars, events.c.ShowMainScreen) assert ars.call_rpc( rpc.s.NewScene.Request(uid(), rpc.s.NewScene.Request.Args("Test scene")), rpc.s.NewScene.Response).result scene_data = event(ars, events.s.OpenScene).data assert scene_data scene = scene_data.scene event(ars, events.s.SceneState) assert ars.call_rpc( rpc.s.AddObjectToScene.Request( uid(), rpc.s.AddObjectToScene.Request.Args("random_actions", RandomActions.__name__)), rpc.s.AddObjectToScene.Response, ).result obj = event(ars, events.s.SceneObjectChanged).data assert obj # ------------------------------------------------------------------------------------------------------------------ assert ars.call_rpc( rpc.p.NewProject.Request( uid(), rpc.p.NewProject.Request.Args(scene.id, "Project name")), rpc.p.NewProject.Response, ).result proj = event(ars, events.p.OpenProject).data assert proj event(ars, events.s.SceneState) assert ars.call_rpc( rpc.p.AddConstant.Request( uid(), rpc.p.AddConstant.Request.Args("min_time", "double", json.dumps(0.45))), rpc.p.AddConstant.Response, ).result c1 = event(ars, events.p.ProjectConstantChanged).data assert c1 assert not ars.call_rpc( rpc.p.AddConstant.Request( uid(), rpc.p.AddConstant.Request.Args("min_time", "double", json.dumps(0.62))), rpc.p.AddConstant.Response, ).result assert not ars.call_rpc( # attempt to update without lock rpc.p.UpdateConstant.Request( uid(), rpc.p.UpdateConstant.Request.Args(c1.id, name="min_time_updated")), rpc.p.UpdateConstant.Response, ).result # ------------------------------------------------------------------------------------------------------------------ # the user opens a menu and then closes it without actually changing anything lock_object(ars, c1.id) assert ars.call_rpc( rpc.p.UpdateConstant.Request(uid(), rpc.p.UpdateConstant.Request.Args( c1.id, name="min_time_1"), dry_run=True), rpc.p.UpdateConstant.Response, ).result assert ars.call_rpc( rpc.p.UpdateConstant.Request(uid(), rpc.p.UpdateConstant.Request.Args( c1.id, name="min_time_2"), dry_run=True), rpc.p.UpdateConstant.Response, ).result unlock_object(ars, c1.id) # ------------------------------------------------------------------------------------------------------------------ lock_object(ars, c1.id) assert ars.call_rpc( rpc.p.UpdateConstant.Request( uid(), rpc.p.UpdateConstant.Request.Args(c1.id, name="min_time_updated")), rpc.p.UpdateConstant.Response, ).result c1u = event(ars, events.p.ProjectConstantChanged).data assert c1u event(ars, events.lk.ObjectsUnlocked) assert c1u.id == c1.id assert c1.name != c1u.name assert c1.type == c1u.type # ------------------------------------------------------------------------------------------------------------------ # try to add and remove assert ars.call_rpc( rpc.p.AddConstant.Request( uid(), rpc.p.AddConstant.Request.Args("min_time_2", "double", json.dumps(0.62))), rpc.p.AddConstant.Response, ).result c2 = event(ars, events.p.ProjectConstantChanged).data assert c2 assert ars.call_rpc( rpc.p.RemoveConstant.Request(uid(), rpc.p.RemoveConstant.Request.Args(c2.id)), rpc.p.RemoveConstant.Response, ).result c2e = event(ars, events.p.ProjectConstantChanged) assert c2e.data assert c2e.data.id == c2.id assert c2e.change_type == c2e.Type.REMOVE # ------------------------------------------------------------------------------------------------------------------ # attempt to add a constant with duplicate name assert not ars.call_rpc( rpc.p.AddConstant.Request( uid(), rpc.p.AddConstant.Request.Args(c1u.name, "double", json.dumps(0.62))), rpc.p.AddConstant.Response, ).result # ------------------------------------------------------------------------------------------------------------------ assert ars.call_rpc( rpc.p.AddActionPoint.Request( uid(), rpc.p.AddActionPoint.Request.Args("ap1", common.Position())), rpc.p.AddActionPoint.Response, ).result ap = event(ars, events.p.ActionPointChanged).data assert ap is not None assert ars.call_rpc( rpc.p.AddAction.Request( uid(), rpc.p.AddAction.Request.Args( ap.id, "test_action", f"{obj.id}/{RandomActions.random_double.__name__}", [ common.ActionParameter( "range_min", common.ActionParameter.TypeEnum.CONSTANT, json.dumps(c1.id)), common.ActionParameter("range_max", "double", "0.55"), ], [common.Flow(outputs=["random_value"])], ), ), rpc.p.AddAction.Response, ).result action = event(ars, events.p.ActionChanged).data assert action assert not ars.call_rpc( rpc.p.RemoveConstant.Request(uid(), rpc.p.RemoveConstant.Request.Args(c1.id)), rpc.p.RemoveConstant.Response).result # ------------------------------------------------------------------------------------------------------------------ # try to execute action using constant parameter assert ars.call_rpc((rpc.s.StartScene.Request(uid())), rpc.s.StartScene.Response).result assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Starting assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Started assert ars.call_rpc( rpc.p.ExecuteAction.Request( uid(), rpc.p.ExecuteAction.Request.Args(action.id)), rpc.p.ExecuteAction.Response) event(ars, events.a.ActionExecution) res = event(ars, events.a.ActionResult) assert res.data assert res.data.action_id == action.id assert not res.data.error assert ars.call_rpc((rpc.s.StopScene.Request(uid())), rpc.s.StopScene.Response).result assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Stopping assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Stopped assert ars.call_rpc( rpc.p.RemoveAction.Request(uid(), rpc.p.IdArgs(action.id)), rpc.p.RemoveAction.Response).result assert event(ars, events.p.ActionChanged).data assert ars.call_rpc( rpc.p.RemoveConstant.Request(uid(), rpc.p.RemoveConstant.Request.Args(c1.id)), rpc.p.RemoveConstant.Response).result event(ars, events.p.ProjectConstantChanged)