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 move_to_pose_cb(req: srpc.r.MoveToPose.Request, ui: WsClient) -> None: glob.LOCK.scene_or_exception() user_name = glob.USERS.user_name(ui) async with ctx_write_lock(req.args.robot_id, user_name, auto_unlock=False): ensure_scene_started() robot_inst = await osa.get_robot_instance(req.args.robot_id) await robot.check_eef_arm(robot_inst, req.args.arm_id, req.args.end_effector_id) await check_feature(robot_inst, Robot.move_to_pose.__name__) await robot.check_robot_before_move(robot_inst) if (req.args.position is None) != (req.args.orientation is None): target_pose = await robot.get_end_effector_pose( robot_inst, req.args.end_effector_id, req.args.arm_id) if req.args.position: target_pose.position = req.args.position elif req.args.orientation: target_pose.orientation = req.args.orientation elif req.args.position is not None and req.args.orientation is not None: target_pose = common.Pose(req.args.position, req.args.orientation) else: raise Arcor2Exception("Position or orientation should be given.") # TODO check if the target pose is reachable (dry_run) asyncio.ensure_future( robot.move_to_pose( robot_inst, req.args.end_effector_id, req.args.arm_id, target_pose, req.args.speed, req.args.safe, user_name, ))
async def _upload_package_cb(req: rpc.UploadPackage.Request, ui: WsClient) -> None: async def _upload_event(path_to_package: str) -> None: summary = await get_summary(path_to_package) evt = events.PackageChanged(summary) evt.change_type = Event.Type.ADD await send_to_clients(evt) logger.info(f"Package '{summary.package_meta.name}' was added.") target_path = os.path.join(PROJECT_PATH, req.args.id) # TODO do not allow if there are manual changes? async with tempfile.TemporaryDirectory() as tmpdirname: zip_path = os.path.join(tmpdirname, "publish.zip") b64_bytes = req.args.data.encode() zip_content = base64.b64decode(b64_bytes) async with aiofiles.open(zip_path, mode="wb") as zip_file: await zip_file.write(zip_content) try: with zipfile.ZipFile(zip_path, "r") as zip_ref: zip_ref.extractall(tmpdirname) except zipfile.BadZipFile as e: logger.error(e) raise Arcor2Exception("Invalid zip file.") await aiofiles.os.remove(zip_path) script_path = os.path.join(tmpdirname, MAIN_SCRIPT_NAME) await check_script(script_path) try: await run_in_executor(shutil.rmtree, target_path, propagate=[FileNotFoundError]) except FileNotFoundError: pass await run_in_executor(shutil.copytree, tmpdirname, target_path) asyncio.create_task(_upload_event(target_path))
async def temporary_package_cb(req: rpc.b.TemporaryPackage.Request, ui: WsClient) -> None: async with glob.LOCK.get_lock(): project = glob.LOCK.project_or_exception() if project.has_changes: raise Arcor2Exception("Project has unsaved changes.") package_id = await build_and_upload_package(project.id, f"Temporary package for project '{project.name}'.") if req.args: paused = req.args.start_paused breakpoints = req.args.breakpoints else: paused = False breakpoints = None asyncio.ensure_future(run_temp_package(package_id, paused, breakpoints)) return None
def check_action_params( scene: CachedScene, project: CachedProject, action: common.Action, object_action: ObjectAction ) -> None: _, action_type = action.parse_type() assert action_type == object_action.name if len(object_action.parameters) != len(action.parameters): raise Arcor2Exception("Unexpected number of parameters.") for req_param in object_action.parameters: param = action.parameter(req_param.name) if param.type == common.ActionParameter.TypeEnum.CONSTANT: const = project.constant(param.str_from_value()) param_meta = object_action.parameter(param.name) if param_meta.type != const.type: raise Arcor2Exception("Param type does not match constant type.") elif param.type == common.ActionParameter.TypeEnum.LINK: parsed_link = param.parse_link() if parsed_link.action_id == action.id: raise Arcor2Exception("Can't use own result as a parameter.") outputs = project.action(parsed_link.action_id).flow(parsed_link.flow_name).outputs assert len(outputs) == len(object_action.returns) param_meta = object_action.parameter(param.name) if param_meta.type != object_action.returns[parsed_link.output_index]: raise Arcor2Exception("Param type does not match action output type.") else: if param.type not in known_parameter_types(): raise Arcor2Exception(f"Parameter {param.name} of action {action.name} has unknown type: {param.type}.") try: plugin_from_type_name(param.type).parameter_value( get_types_dict(), scene, project, action.id, param.name ) except ParameterPluginException as e: raise Arcor2Exception(f"Parameter {param.name} of action {action.name} has invalid value. {str(e)}")
async def scene_object_usage_request_cb(req: rpc.scene.SceneObjectUsageRequest, ui: WsClient) -> \ rpc.scene.SceneObjectUsageResponse: """ Works for both services and objects. :param req: :return: """ assert glob.SCENE if not (any(obj.id == req.args.id for obj in glob.SCENE.objects) or any(srv.type == req.args.id for srv in glob.SCENE.services)): raise Arcor2Exception("Unknown ID.") resp = rpc.scene.SceneObjectUsageResponse() async for project in projects_using_object(glob.SCENE.id, req.args.id): resp.data.add(project.id) return resp
async def update_object_pose_cb(req: rpc.scene.UpdateObjectPoseRequest, ui: WsClient) -> None: assert glob.SCENE obj = glob.SCENE.object(req.args.object_id) if glob.OBJECT_TYPES[obj.type].needs_services: raise Arcor2Exception("Can't manipulate object created by service.") obj.pose = req.args.pose glob.SCENE_OBJECT_INSTANCES[req.args.object_id].pose = req.args.pose glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=obj))) OBJECTS_WITH_UPDATED_POSE.add(obj.id) return None
async def update_lock(self, obj_id: str, owner: str, upgrade_type: UpdateType) -> None: """Upgrades lock to locked whole tree or downgrades lock to simple object lock. :param obj_id: objects which is locked and updated :param owner: owner of current lock :param upgrade_type: one of available type """ root_id = await self.get_root_id(obj_id) async with self._lock: if root_id not in self._locked_objects: raise LockingException(self.ErrMessages.NOT_LOCKED.value) lock_record = self._get_lock_record(root_id) if upgrade_type == UpdateType.TREE: lock_record.check_upgrade(obj_id, owner) lock_record.tree = True to_notify = self.get_all_children(root_id) to_notify.add(root_id) to_notify.remove(obj_id) evt = LockEventData(to_notify, owner, True) self._upsert_user_locked_objects(owner, to_notify) elif upgrade_type == UpdateType.OBJECT: lock_record.check_downgrade(obj_id, owner) lock_record.tree = False to_notify = self.get_all_children(root_id) to_notify.add(root_id) to_notify -= {obj_id} evt = LockEventData(to_notify, owner) self._remove_user_locked_objects(owner, to_notify) else: raise Arcor2Exception("Unknown type of lock upgrade") self.notifications_q.put_nowait(evt)
def detect_ap_loop(ap: common.BareActionPoint, new_parent_id: str) -> None: assert glob.PROJECT visited_ids: Set[str] = set() ap = copy.deepcopy(ap) ap.parent = new_parent_id while True: if ap.id in visited_ids: raise Arcor2Exception("Loop detected!") visited_ids.add(ap.id) if ap.parent is None: break # type: ignore # this is certainly reachable try: ap = glob.PROJECT.bare_action_point(ap.parent) except Arcor2Exception: break
async def get_summary(path: str) -> PackageSummary: if not os.path.isfile(os.path.join(path, MAIN_SCRIPT_NAME)): raise Arcor2Exception("Package does not contain main script.") package_dir = os.path.basename(path) package_meta = read_package_meta(package_dir) try: with open(os.path.join(path, "data", "project.json")) as project_file: project = common.Project.from_json(project_file.read()) except (ValidationError, IOError) as e: logger.error(f"Failed to read/parse project file of {package_dir}: {e}") return PackageSummary(package_dir, package_meta) modified = project.modified if not modified: modified = datetime.fromtimestamp(0, tz=timezone.utc) return PackageSummary(package_dir, package_meta, ProjectMeta(project.id, project.name, modified))
async def get_project(project_id: str) -> Project: try: project = _projects[project_id] assert project.modified except KeyError: project = await ps.get_project(project_id) _projects[project_id] = project else: await _update_list(ps.get_projects, _projects_list, _projects) if project_id not in _projects_list.listing: _projects.pop(project_id, None) raise Arcor2Exception("Project removed externally.") # project in cache is outdated if project.modified < _projects_list.listing[project_id].modified: project = await ps.get_project(project_id) _projects[project_id] = project return project
async def get_scene(scene_id: str) -> Scene: try: scene = _scenes[scene_id] assert scene.modified except KeyError: scene = await ps.get_scene(scene_id) _scenes[scene_id] = scene else: await _update_list(ps.get_scenes, _scenes_list, _scenes) if scene_id not in _scenes_list.listing: _scenes.pop(scene_id, None) raise Arcor2Exception("Scene removed externally.") # scene in cache is outdated if scene.modified < _scenes_list.listing[scene_id].modified: scene = await ps.get_scene(scene_id) _scenes[scene_id] = scene return scene
async def close_scene_cb(req: srpc.s.CloseScene.Request, ui: WsClient) -> None: """Closes scene on the server. :param req: :return: """ assert glob.SCENE if not req.args.force and glob.SCENE.has_changes(): raise Arcor2Exception("Scene has unsaved changes.") can_modify_scene() # can't close scene while started if req.dry_run: return None scene_id = glob.SCENE.id glob.SCENE = None glob.OBJECTS_WITH_UPDATED_POSE.clear() asyncio.ensure_future(notify_scene_closed(scene_id))
async def open_scene(scene_id: str) -> None: glob.SCENE = await storage.get_scene(scene_id) try: for srv in glob.SCENE.services: await add_service_to_scene(srv) for obj in glob.SCENE.objects: await add_object_to_scene(obj, add_to_scene=False, srv_obj_ok=True) except Arcor2Exception as e: await clear_scene() raise Arcor2Exception(f"Failed to open scene. {e.message}") from e assert {srv.type for srv in glob.SCENE.services} == glob.SERVICES_INSTANCES.keys() assert {obj.id for obj in glob.SCENE.objects } == glob.SCENE_OBJECT_INSTANCES.keys()
async def _upload_package_cb(req: rpc.UploadPackage.Request, ui: WsClient) -> None: target_path = os.path.join(PROJECT_PATH, req.args.id) # TODO do not allow if there are manual changes? with tempfile.TemporaryDirectory() as tmpdirname: zip_path = os.path.join(tmpdirname, "publish.zip") b64_bytes = req.args.data.encode() zip_content = base64.b64decode(b64_bytes) with open(zip_path, "wb") as zip_file: zip_file.write(zip_content) try: with zipfile.ZipFile(zip_path, "r") as zip_ref: zip_ref.extractall(tmpdirname) except zipfile.BadZipFile as e: logger.error(e) raise Arcor2Exception("Invalid zip file.") os.remove(zip_path) try: shutil.rmtree(target_path) except FileNotFoundError: pass shutil.copytree(tmpdirname, target_path) script_path = os.path.join(target_path, MAIN_SCRIPT_NAME) check_script(script_path) evt = events.PackageChanged(await get_summary(target_path)) evt.change_type = Event.Type.ADD asyncio.ensure_future(send_to_clients(evt)) return None
def move_to_pose( self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, arm_id: Optional[str] = None ) -> None: """Move given robot's end effector to the selected pose. :param end_effector_id: :param target_pose: :param speed: :param safe: :return: """ if end_effector_id not in self.get_end_effectors_ids(arm_id): raise Arcor2Exception("Unknown end effector.") assert arm_id speed = min(max(0.0, speed), 1.0) with self._move_lock: time.sleep(1.0 - speed) self._poses[arm_id][end_effector_id] = tr.make_pose_rel(self.pose, target_pose)
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
def __init__( self, obj_id: str, name: str, pose: Pose, collision_model: Models, settings: UrlSettings, ) -> None: super(ConveyorBelt, self).__init__(obj_id, name, pose, collision_model, settings) iter: int = 0 while True: if self._started(): break time.sleep(0.1) iter += 1 if iter > 10: raise Arcor2Exception( "Failed to connect to the Dobot Service.")
async def get_object_type(object_type_id: str) -> ObjectType: try: ot = _object_types[object_type_id] assert ot.modified except KeyError: ot = await ps.get_object_type(object_type_id) _object_types[object_type_id] = ot else: await _update_list(ps.get_object_type_ids, _object_type_list, _object_types) if object_type_id not in _object_type_list.listing: _object_types.pop(object_type_id, None) raise Arcor2Exception("ObjectType removed externally.") # ObjectType in cache is outdated if ot.modified < _object_type_list.listing[object_type_id].modified: ot = await ps.get_object_type(object_type_id) _object_types[object_type_id] = ot return ot
async def object_aiming_add_point_cb( req: srpc.o.ObjectAimingAddPoint.Request, ui: WsClient) -> srpc.o.ObjectAimingAddPoint.Response: scene = glob.LOCK.scene_or_exception() fo, user_name = await object_aiming_check(ui) pt_idx = req.args.point_idx scene_obj = scene.object(fo.obj_id) obj_type = glob.OBJECT_TYPES[scene_obj.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.") robot_id, end_effector, arm_id = fo.robot.as_tuple() robot_inst = get_robot_instance(robot_id) r = srpc.o.ObjectAimingAddPoint.Response() r.data = r.Data(finished_indexes=list(fo.poses.keys())) if not req.dry_run: fo.poses[pt_idx] = await get_end_effector_pose(robot_inst, end_effector, arm_id) r.data = r.Data(finished_indexes=list(fo.poses.keys())) logger.info( f"{user_name} just aimed index {pt_idx} for {scene_obj.name}. Done indexes: {r.data.finished_indexes}." ) return r
async def _initialize_server() -> None: exe_version = await exe.manager_request( rpc.common.VersionRequest(uuid.uuid4().int)) assert isinstance(exe_version, rpc.common.VersionResponse) """ Following check is especially useful when running server/execution in Docker containers. Then it might easily happen that one tries to use different versions together. """ try: hlp.check_compatibility(arcor2.api_version(), exe_version.data.version) except Arcor2Exception as e: raise Arcor2Exception( "ARServer/Execution API_VERSION mismatch.") from e while True: # wait until Project service becomes available try: await storage.get_projects() break except storage.PersistentStorageException as e: print(e.message) await asyncio.sleep(1) # this has to be done sequentially as objects might depend on services so (all) services has to be known first await osa.get_service_types() await osa.get_object_types() await osa.get_object_actions() bound_handler = functools.partial(hlp.server, logger=glob.logger, register=register, unregister=unregister, rpc_dict=RPC_DICT, event_dict=EVENT_DICT, verbose=glob.VERBOSE) await glob.logger.info("Server initialized.") await asyncio.wait([websockets.serve(bound_handler, '0.0.0.0', glob.PORT)])
async def update_action_point_joints_cb( req: srpc.p.UpdateActionPointJoints.Request, ui: WsClient) -> None: assert glob.SCENE assert glob.PROJECT robot_joints = glob.PROJECT.joints(req.args.joints_id) if {joint.name for joint in req.args.joints } != {joint.name for joint in robot_joints.joints}: raise Arcor2Exception("Joint names does not match the robot.") # TODO maybe joints values should be normalized? To <0, 2pi> or to <-pi, pi>? robot_joints.joints = req.args.joints robot_joints.is_valid = True glob.PROJECT.update_modified() evt = sevts.p.JointsChanged(robot_joints) evt.change_type = Event.Type.UPDATE asyncio.ensure_future(notif.broadcast_event(evt)) return None
async def hand_teaching_mode_cb(req: srpc.r.HandTeachingMode.Request, ui: WsClient) -> None: glob.LOCK.scene_or_exception() ensure_scene_started() robot_inst = await osa.get_robot_instance(req.args.robot_id) # in this case, method name does not correspond to feature name await check_feature(robot_inst, "hand_teaching") hand_teaching_mode = await run_in_executor(robot_inst.get_hand_teaching_mode) if req.args.enable == hand_teaching_mode: raise Arcor2Exception("That's the current state.") await ensure_locked(req.args.robot_id, ui) if req.dry_run: return await run_in_executor(robot_inst.set_hand_teaching_mode, req.args.enable) evt = HandTeachingMode(HandTeachingMode.Data(req.args.robot_id, req.args.enable)) asyncio.ensure_future(notif.broadcast_event(evt))
def check_object(scene: CachedScene, obj: SceneObject, new_one: bool = False) -> None: """Checks if object can be added into the scene.""" assert not obj.children if obj.type not in glob.OBJECT_TYPES: raise Arcor2Exception("Unknown object type.") obj_type = glob.OBJECT_TYPES[obj.type] if obj_type.meta.disabled: raise Arcor2Exception("Object type disabled.") check_object_parameters(obj_type, obj.parameters) # TODO check whether object needs parent and if so, if the parent is in scene and parent_id is set if obj_type.meta.needs_parent_type: pass if obj_type.meta.has_pose and obj.pose is None: raise Arcor2Exception("Object requires pose.") if not obj_type.meta.has_pose and obj.pose is not None: raise Arcor2Exception("Object do not have pose.") if obj_type.meta.abstract: raise Arcor2Exception("Cannot instantiate abstract type.") if new_one: if obj.id in scene.object_ids: raise Arcor2Exception( "Object/service with that id already exists.") if obj.name in scene.object_names(): raise Arcor2Exception("Name is already used.") hlp.is_valid_identifier(obj.name)
def get_by_id( self, obj_id: str ) -> Union[ cmn.SceneObject, cmn.BareActionPoint, cmn.NamedOrientation, cmn.ProjectRobotJoints, cmn.Action, cmn.ProjectParameter, ]: """Retrive object by it's ID.""" if self.project: try: return self.project.get_by_id(obj_id) except CachedProjectException: ... if self.scene: # TODO update with scene object hierarchy if obj_id in self.scene.object_ids: return self.scene.object(obj_id) raise Arcor2Exception(f"Object ID {obj_id} not found.")
async def stop_package_cb(req: rpc.StopPackage.Request, ui: WsClient) -> None: global PACKAGE_INFO_EVENT global RUNNING_PACKAGE_ID if not process_running(): raise Arcor2Exception("Project not running.") assert PROCESS is not None assert TASK is not None await package_state( PackageState( PackageState.Data(PackageState.Data.StateEnum.STOPPING, RUNNING_PACKAGE_ID))) logger.info("Terminating process") PROCESS.send_signal( signal.SIGINT) # the same as when a user presses ctrl+c logger.info("Waiting for process to finish...") await asyncio.wait([TASK]) PACKAGE_INFO_EVENT = None RUNNING_PACKAGE_ID = None
def check_for_loops(parent: LogicContainer, first_action_id: Optional[str] = None) -> None: """Finds loops in logic. Can process even unfinished logic, when first action id is provided. :param parent: :param first_action_id: :return: """ def _check_for_loops(action: Action, visited_actions: Set[str]) -> None: if action.id in visited_actions: raise Arcor2Exception("Loop detected!") visited_actions.add(action.id) _, outputs = parent.action_io(action.id) for output in outputs: if output.end == output.END: continue # each possible execution path have its own set of visited actions _check_for_loops(parent.action(output.end), visited_actions.copy()) if first_action_id is None: try: first_action_id = parent.first_action_id() except CachedProjectException as e: raise Arcor2Exception("Can't check unfinished logic.") from e first_action = parent.action(first_action_id) visited_actions: Set[str] = set() _check_for_loops(first_action, visited_actions)
async def stop_package_cb(req: rpc.StopPackage.Request, ui: WsClient) -> None: async def _terminate_task() -> None: global PACKAGE_INFO_EVENT global RUNNING_PACKAGE_ID assert PROCESS assert TASK logger.info("Terminating process") PROCESS.send_signal(signal.SIGINT) # the same as when a user presses ctrl+c logger.info("Waiting for process to finish...") await asyncio.wait([TASK]) PACKAGE_INFO_EVENT = None RUNNING_PACKAGE_ID = None if PACKAGE_STATE_EVENT.data.state not in PackageState.RUN_STATES: raise Arcor2Exception("Package not running.") assert process_running() await package_state(PackageState(PackageState.Data(PackageState.Data.StateEnum.STOPPING, RUNNING_PACKAGE_ID))) asyncio.create_task(_terminate_task())
async def remove_constant_cb(req: srpc.p.RemoveConstant.Request, ui: WsClient) -> None: assert glob.PROJECT assert glob.SCENE const = glob.PROJECT.constant(req.args.constant_id) # check for usage for act in glob.PROJECT.actions: for param in act.parameters: if param.type == common.ActionParameter.TypeEnum.CONSTANT and param.str_from_value( ) == const.id: raise Arcor2Exception("Constant used as action parameter.") if req.dry_run: return glob.PROJECT.remove_constant(const.id) evt = sevts.p.ProjectConstantChanged(const) evt.change_type = Event.Type.REMOVE asyncio.ensure_future(notif.broadcast_event(evt)) return None
async def add_action_point_cb(req: rpc.project.AddActionPointRequest, ui: WsClient) -> None: assert glob.SCENE assert glob.PROJECT unique_name(req.args.name, glob.PROJECT.action_points_names) check_ap_parent(req.args.parent) if not hlp.is_valid_identifier(req.args.name): raise Arcor2Exception("Name has to be valid Python identifier.") if req.dry_run: return None ap = common.ProjectActionPoint(common.uid(), req.args.name, req.args.position, req.args.parent) glob.PROJECT.action_points.append(ap) glob.PROJECT.update_modified() asyncio.ensure_future( notif.broadcast_event( events.ActionPointChanged(events.EventType.ADD, data=ap))) return None