async def get_robot_arms_cb(req: srpc.r.GetRobotArms.Request, ui: WsClient) -> srpc.r.GetRobotArms.Response: glob.LOCK.scene_or_exception() async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)): ensure_scene_started() return srpc.r.GetRobotArms.Response(data=await robot.get_arms(get_robot_instance(req.args.robot_id)))
async def register_for_robot_event_cb(req: srpc.r.RegisterForRobotEvent.Request, ui: WsClient) -> None: glob.LOCK.scene_or_exception() async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)): ensure_scene_started() # check if robot exists robot_inst = get_robot_instance(req.args.robot_id) if req.args.what == req.args.RegisterEnum.JOINTS: await register( req, robot_inst, ui, ROBOT_JOINTS_TASKS, glob.ROBOT_JOINTS_REGISTERED_UIS, robot_joints_event ) elif req.args.what == req.args.RegisterEnum.EEF_POSE: if isinstance(robot_inst, MultiArmRobot): for arm_id in await robot.get_arms(robot_inst): if await robot.get_end_effectors(robot_inst, arm_id): break else: raise Arcor2Exception("Robot does not have any end effector.") elif not await robot.get_end_effectors(robot_inst, None): raise Arcor2Exception("Robot does not have any end effector.") await register(req, robot_inst, ui, EEF_POSE_TASKS, glob.ROBOT_EEF_REGISTERED_UIS, robot_eef_pose_event) else: raise Arcor2Exception(f"Option '{req.args.what.value}' not implemented.") return None
async def projects_with_scene_cb( req: srpc.s.ProjectsWithScene.Request, ui: WsClient) -> srpc.s.ProjectsWithScene.Response: async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)): resp = srpc.s.ProjectsWithScene.Response() resp.data = await associated_projects(req.args.id) return resp
async def get_end_effector_pose_cb( req: srpc.r.GetEndEffectorPose.Request, ui: WsClient) -> srpc.r.GetEndEffectorPose.Response: glob.LOCK.scene_or_exception() async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)): ensure_scene_started() return srpc.r.GetEndEffectorPose.Response( data=await robot.get_end_effector_pose( await osa.get_robot_instance(req.args.robot_id), req.args.end_effector_id, req.args.arm_id))
async def fk_cb(req: srpc.r.ForwardKinematics.Request, ui: WsClient) -> srpc.r.ForwardKinematics.Response: glob.LOCK.scene_or_exception() async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)): ensure_scene_started() robot_inst = get_robot_instance(req.args.robot_id) await check_feature(robot_inst, Robot.forward_kinematics.__name__) pose = await robot.fk(robot_inst, req.args.end_effector_id, req.args.arm_id, req.args.joints) resp = srpc.r.ForwardKinematics.Response() resp.data = pose return resp
async def focus_object_start_cb(req: srpc.o.FocusObjectStart.Request, ui: WsClient) -> None: # TODO this event should take long time, notify UI that robot is locked async with ctx_read_lock([req.args.object_id, req.args.robot.robot_id], glob.USERS.user_name(ui), auto_unlock=False): scene = glob.LOCK.scene_or_exception() if glob.LOCK.project: raise Arcor2Exception("Project has to be closed first.") ensure_scene_started() obj_id = req.args.object_id if obj_id in FOCUS_OBJECT_ROBOT: raise Arcor2Exception("Focusing already started.") if obj_id not in glob.SCENE_OBJECT_INSTANCES: raise Arcor2Exception("Unknown object.") inst = await osa.get_robot_instance(req.args.robot.robot_id) await check_eef_arm(inst, req.args.robot.arm_id, req.args.robot.end_effector) robot_type = glob.OBJECT_TYPES[inst.__class__.__name__] assert robot_type.robot_meta obj_type = glob.OBJECT_TYPES[scene.object(obj_id).type].meta if not obj_type.has_pose: raise Arcor2Exception("Only available for objects with pose.") if not obj_type.object_model or obj_type.object_model.type != Model3dType.MESH: raise Arcor2Exception( "Only available for objects with mesh model.") assert obj_type.object_model.mesh focus_points = obj_type.object_model.mesh.focus_points if not focus_points: raise Arcor2Exception("focusPoints not defined for the mesh.") FOCUS_OBJECT_ROBOT[req.args.object_id] = req.args.robot FOCUS_OBJECT[obj_id] = {} glob.logger.info(f"Start of focusing for {obj_id}.") return None
async def ik_cb(req: srpc.r.InverseKinematics.Request, ui: WsClient) -> srpc.r.InverseKinematics.Response: glob.LOCK.scene_or_exception() async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)): ensure_scene_started() robot_inst = await osa.get_robot_instance(req.args.robot_id) await check_feature(robot_inst, Robot.inverse_kinematics.__name__) joints = await robot.ik( robot_inst, req.args.end_effector_id, req.args.pose, req.args.start_joints, req.args.avoid_collisions ) resp = srpc.r.InverseKinematics.Response() resp.data = joints return resp
async def focus_object_cb(req: srpc.o.FocusObject.Request, ui: WsClient) -> srpc.o.FocusObject.Response: async with ctx_read_lock(req.args.object_id, glob.USERS.user_name(ui), auto_unlock=False): scene = glob.LOCK.scene_or_exception() if glob.LOCK.project: raise Arcor2Exception("Project has to be closed first.") obj_id = req.args.object_id pt_idx = req.args.point_idx if obj_id not in glob.SCENE_OBJECT_INSTANCES: raise Arcor2Exception("Unknown object_id.") obj_type = glob.OBJECT_TYPES[scene.object(obj_id).type].meta assert obj_type.has_pose assert obj_type.object_model assert obj_type.object_model.mesh focus_points = obj_type.object_model.mesh.focus_points assert focus_points if pt_idx < 0 or pt_idx > len(focus_points) - 1: raise Arcor2Exception("Index out of range.") if obj_id not in FOCUS_OBJECT: glob.logger.info(f"Start of focusing for {obj_id}.") FOCUS_OBJECT[obj_id] = {} robot_id, end_effector, arm_id = FOCUS_OBJECT_ROBOT[obj_id].as_tuple() FOCUS_OBJECT[obj_id][pt_idx] = await get_end_effector_pose( await osa.get_robot_instance(robot_id), end_effector, arm_id) r = srpc.o.FocusObject.Response() r.data = r.Data(finished_indexes=list(FOCUS_OBJECT[obj_id].keys())) return r
async def action_param_values_cb( req: srpc.o.ActionParamValues.Request, ui: WsClient) -> srpc.o.ActionParamValues.Response: glob.LOCK.scene_or_exception() async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)): ensure_scene_started() inst = get_instance(req.args.id) parent_params = {} for pp in req.args.parent_params: parent_params[pp.id] = pp.value try: method_name, required_parent_params = inst.DYNAMIC_PARAMS[ req.args.param_id] except KeyError: raise Arcor2Exception( "Unknown parameter or values not constrained.") if parent_params.keys() != required_parent_params: raise Arcor2Exception("Not all required parent params were given.") # TODO validate method parameters vs parent_params (check types)? resp = srpc.o.ActionParamValues.Response() try: method = getattr(inst, method_name) except AttributeError: glob.logger.error( f"Unable to get values for parameter {req.args.param_id}, " f"object/service {inst.id} has no method named {method_name}.") raise Arcor2Exception("System error.") # TODO update hlp.run_in_executor to support kwargs resp.data = await asyncio.get_event_loop().run_in_executor( None, functools.partial(method, **parent_params)) return resp
async def scene_object_usage_request_cb( req: srpc.s.SceneObjectUsage.Request, ui: WsClient) -> srpc.s.SceneObjectUsage.Response: """Works for both services and objects. :param req: :return: """ scene = glob.LOCK.scene_or_exception() async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)): if not (any(obj.id == req.args.id for obj in scene.objects)): raise Arcor2Exception("Unknown ID.") resp = srpc.s.SceneObjectUsage.Response() resp.data = set() async for project in projects_using_object(scene.id, req.args.id): resp.data.add(project.id) return resp
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)