async def object_aiming_done_cb(req: srpc.o.ObjectAimingDone.Request, ui: WsClient) -> None: """Calls scene service to get a new pose for the object. In case of success, robot and object are kept locked, unlocking is responsibility of ui. On failure, UI may do another attempt or call ObjectAimingCancel. :param req: :param ui: :return: """ scene = glob.LOCK.scene_or_exception() fo, user_name = await object_aiming_check(ui) obj_type = glob.OBJECT_TYPES[scene.object(fo.obj_id).type].meta assert obj_type.object_model assert obj_type.object_model.mesh focus_points = obj_type.object_model.mesh.focus_points assert focus_points if len(fo.poses) < len(focus_points): raise Arcor2Exception( f"Only {len(fo.poses)} points were done out of {len(focus_points)}." ) obj = scene.object(fo.obj_id) assert obj.pose obj_inst = get_instance(fo.obj_id, CollisionObject) if req.dry_run: return fp: list[Position] = [] rp: list[Position] = [] for idx, pose in fo.poses.items(): fp.append(focus_points[idx].position) rp.append(pose.position) mfa = MeshFocusAction(fp, rp) logger.debug(f"Attempt to aim object {obj_inst.name}, data: {mfa}") try: new_pose = await scene_srv.focus(mfa) # TODO how long does it take? except scene_srv.SceneServiceException as e: logger.error(f"Aiming failed with: {e}, mfa: {mfa}.") raise Arcor2Exception(f"Aiming failed. {str(e)}") from e logger.info(f"Done aiming for {obj_inst.name}.") _objects_being_aimed.pop(user_name, None) asyncio.create_task( update_scene_object_pose(scene, obj, new_pose, obj_inst)) return None
async def project_manager_client(handle_manager_incoming_messages) -> None: while True: logger.info("Attempting connection to manager...") try: async with websockets.connect(EXE_URL) as manager_client: # type: ignore # TODO not sure what is wrong logger.info("Connected to manager.") future = asyncio.ensure_future(handle_manager_incoming_messages(manager_client)) while True: if future.done(): break try: msg = await asyncio.wait_for(MANAGER_RPC_REQUEST_QUEUE.get(), 1.0) except asyncio.TimeoutError: continue try: await manager_client.send(msg.to_json()) except websockets.exceptions.ConnectionClosed: await MANAGER_RPC_REQUEST_QUEUE.put(msg) break except ConnectionRefusedError as e: logger.error(e) await asyncio.sleep(delay=1.0)
async def robot_joints_event(robot_inst: Robot) -> None: logger.info(f"Sending joints for {robot_inst.name} started.") while scene_started() and glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]: start = time.monotonic() try: evt = RobotJoints(RobotJoints.Data(robot_inst.id, (await robot.get_robot_joints(robot_inst, None, True)))) except Arcor2Exception as e: logger.error(f"Failed to get joints for {robot_inst.name}. {str(e)}") await asyncio.sleep(1) continue evt_json = evt.to_json() await asyncio.gather( *[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]] ) end = time.monotonic() await asyncio.sleep(EVENT_PERIOD - (end - start)) ROBOT_JOINTS_TASKS.pop(robot_inst.id, None) # TODO notify UIs that registration was cancelled glob.ROBOT_JOINTS_REGISTERED_UIS.pop(robot_inst.id, None) logger.info(f"Sending joints for {robot_inst.name} stopped.")
async def _delete_model(obj_type: ObjectTypeData) -> None: # do not care so much if delete_model fails if not obj_type.meta.object_model: return try: await storage.delete_model(obj_type.meta.object_model.model().id) except storage.ProjectServiceException as e: logger.error(str(e))
async def _move_to_joints(robot_inst: Robot, joints: list[common.Joint], speed: float, safe: bool, arm_id: Optional[str]) -> None: # TODO newly connected interface should be notified somehow (general solution for such cases would be great!) try: await hlp.run_in_executor( robot_inst.move_to_joints, *prepare_args(robot_inst, [joints, speed, safe], arm_id)) except Arcor2Exception as e: logger.error(f"Robot movement failed with: {str(e)}") raise
async def robot_eef_pose_event(robot_inst: Robot) -> None: eefs: dict[Optional[str], set[str]] = {} try: try: for arm_id in await robot.get_arms(robot_inst): eefs[arm_id] = await robot.get_end_effectors(robot_inst, arm_id) except robot.SingleArmRobotException: eefs = {None: await robot.get_end_effectors(robot_inst, None)} except Arcor2Exception as e: logger.error(f"Failed to start sending poses for {robot_inst.name}. {str(e)}") else: logger.info(f"Sending poses for {robot_inst.name} started. Arms/EEFs: {eefs}.") while scene_started() and glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]: start = time.monotonic() evt = RobotEef(RobotEef.Data(robot_inst.id)) try: evt.data.end_effectors = await asyncio.gather( *[eef_pose(robot_inst, eef_id, arm_id) for arm_id in eefs.keys() for eef_id in eefs[arm_id]] ) except Arcor2Exception as e: logger.error(f"Failed to get eef pose for {robot_inst.name}. {str(e)}") await asyncio.sleep(1) continue evt_json = evt.to_json() await asyncio.gather( *[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]] ) end = time.monotonic() await asyncio.sleep(EVENT_PERIOD - (end - start)) EEF_POSE_TASKS.pop(robot_inst.id, None) # TODO notify UIs that registration was cancelled glob.ROBOT_EEF_REGISTERED_UIS.pop(robot_inst.id, None) logger.info(f"Sending poses for {robot_inst.name} stopped.")
async def _move_to_pose( robot_inst: Robot, end_effector_id: str, arm_id: Optional[str], pose: common.Pose, speed: float, safe: bool, linear: bool, ) -> None: # TODO newly connected interface should be notified somehow (general solution for such cases would be great!) try: await hlp.run_in_executor( robot_inst.move_to_pose, *prepare_args(robot_inst, [end_effector_id, pose, speed, safe, linear], arm_id)) except Arcor2Exception as e: logger.error(f"Robot movement failed with: {str(e)}") raise
async def get_robot_meta(obj_type: ObjectTypeData) -> None: if obj_type.meta.disabled: raise Arcor2Exception("Disabled object type.") assert obj_type.type_def is not None if not issubclass(obj_type.type_def, Robot): raise Arcor2Exception("Not a robot.") obj_type.robot_meta = RobotMeta( obj_type.meta.type, obj_type.type_def.robot_type, issubclass(obj_type.type_def, MultiArmRobot)) # TODO fix mypy issue 'Can only assign concrete classes to a variable of type "Type[Robot]"' base_class: type[ Robot] = MultiArmRobot if obj_type.robot_meta.multi_arm else Robot # type: ignore # TODO automate this somehow obj_type.robot_meta.features.move_to_pose = _feature( obj_type.type_def, Robot.move_to_pose.__name__, base_class) obj_type.robot_meta.features.move_to_joints = _feature( obj_type.type_def, Robot.move_to_joints.__name__, base_class) obj_type.robot_meta.features.stop = _feature(obj_type.type_def, Robot.stop.__name__, base_class) obj_type.robot_meta.features.inverse_kinematics = _feature( obj_type.type_def, Robot.inverse_kinematics.__name__, base_class) obj_type.robot_meta.features.forward_kinematics = _feature( obj_type.type_def, Robot.forward_kinematics.__name__, base_class) obj_type.robot_meta.features.hand_teaching = _feature( obj_type.type_def, Robot.set_hand_teaching_mode.__name__, base_class) if urdf_name := obj_type.type_def.urdf_package_name: if urdf_name not in await ps.files_ids(): logger.error( f"URDF package {urdf_name} for {obj_type.meta.type} does not exist." ) else: obj_type.robot_meta.urdf_package_filename = urdf_name
async def execute_action(action_method: Callable, params: list[Any]) -> None: assert glob.RUNNING_ACTION await notif.broadcast_event( ActionExecution(ActionExecution.Data(glob.RUNNING_ACTION))) evt = ActionResult(ActionResult.Data(glob.RUNNING_ACTION)) try: action_result = await hlp.run_in_executor(action_method, *params) except Arcor2Exception as e: logger.error( f"Failed to run method {action_method.__name__} with params {params}. {str(e)}" ) logger.debug(str(e), exc_info=True) evt.data.error = str(e) else: if action_result is not None: glob.PREV_RESULTS[glob.RUNNING_ACTION] = action_result try: evt.data.results = results_to_json(action_result) except Arcor2Exception: logger.error( f"Method {action_method.__name__} returned unsupported type of result: {action_result}." ) if glob.RUNNING_ACTION is None: # action was cancelled, do not send any event return # type: ignore # action could be cancelled during its execution glob.RUNNING_ACTION = None glob.RUNNING_ACTION_PARAMS = None await notif.broadcast_event(evt)
async def start_scene(scene: CachedScene) -> None: """Creates instances of scene objects.""" async def _start_scene() -> bool: logger.info(f"Starting the {scene.name} scene.") await set_scene_state(SceneState.Data.StateEnum.Starting) # in order to prepare a clear environment try: # stop deletes all configurations and clears all collisions await scene_srv.stop() except Arcor2Exception: logger.exception("Failed to prepare for start.") await set_scene_state(SceneState.Data.StateEnum.Stopped, "Failed to prepare for start.") return False object_overrides: dict[str, list[Parameter]] = {} if glob.LOCK.project: object_overrides = glob.LOCK.project.overrides # object initialization could take some time - let's do it in parallel tasks = [ asyncio.ensure_future( create_object_instance( obj, object_overrides[obj.id] if obj.id in object_overrides else None)) for obj in scene.objects ] try: await asyncio.gather(*tasks) except Arcor2Exception as e: for t in tasks: t.cancel() # TODO maybe it would be better to let them finish? logger.exception("Failed to create instances.") await stop_scene(scene, str(e), already_locked=True) return False try: await scene_srv.start() except Arcor2Exception as e: logger.exception("Failed to go online.") await stop_scene(scene, str(e), already_locked=True) return False await set_scene_state(SceneState.Data.StateEnum.Started) return True to_lock = [glob.LOCK.SpecialValues.SCENE] if glob.LOCK.project: to_lock.append(glob.LOCK.SpecialValues.PROJECT) try: async with ctx_write_lock(to_lock, glob.LOCK.Owners.SERVER): ret = await _start_scene() except CannotLock: logger.warning( f"Failed attempt to start the scene. Can't lock {to_lock}.") return except Arcor2Exception as e: logger.error(f"Failed to start the scene. {str(e)}") assert ret == scene_started() assert ret == await scene_srv.started() if ret: logger.info("Scene started. Enjoy!")
async def stop_scene(scene: CachedScene, message: Optional[str] = None, already_locked: bool = False) -> None: """Destroys scene object instances.""" async def _stop_scene() -> None: await set_scene_state(SceneState.Data.StateEnum.Stopping, message) try: await scene_srv.stop() except Arcor2Exception as e: logger.exception("Failed to go offline.") await set_scene_state(SceneState.Data.StateEnum.Started, str(e)) return try: await asyncio.gather(*[ cleanup_object(obj) for obj in glob.SCENE_OBJECT_INSTANCES.values() ]) except Arcor2Exception as e: logger.exception("Exception occurred while cleaning up objects.") await set_scene_state(SceneState.Data.StateEnum.Stopped, str(e)) else: await set_scene_state(SceneState.Data.StateEnum.Stopped) glob.SCENE_OBJECT_INSTANCES.clear() glob.PREV_RESULTS.clear() if already_locked: logger.info( f"Stopping the {scene.name} scene after unsuccessful start.") assert await glob.LOCK.is_write_locked(glob.LOCK.SpecialValues.SCENE, glob.LOCK.Owners.SERVER) assert (glob.LOCK.project is not None) == await glob.LOCK.is_write_locked( glob.LOCK.SpecialValues.PROJECT, glob.LOCK.Owners.SERVER) await _stop_scene() else: to_lock = [glob.LOCK.SpecialValues.SCENE] if glob.LOCK.project: to_lock.append(glob.LOCK.SpecialValues.PROJECT) try: async with ctx_write_lock(to_lock, glob.LOCK.Owners.SERVER): logger.info(f"Stopping the {scene.name} scene.") await _stop_scene() except CannotLock: logger.warning( f"Failed attempt to stop the scene. Can't lock {to_lock}.") return except Arcor2Exception as e: logger.error(f"Failed to stop the scene. {str(e)}") return assert not scene_started() assert not await scene_srv.started() logger.info("Scene stopped.")
async def get_object_data(object_types: ObjectTypeDict, obj_id: str) -> None: logger.debug(f"Processing {obj_id}.") if obj_id in object_types: logger.debug(f"{obj_id} already processed, skipping...") return obj_iddesc = await storage.get_object_type_iddesc(obj_id) if obj_id in glob.OBJECT_TYPES: assert obj_iddesc.modified assert glob.OBJECT_TYPES[ obj_id].meta.modified, f"Object {obj_id} does not have 'modified' in its meta." if obj_iddesc.modified == glob.OBJECT_TYPES[obj_id].meta.modified: logger.debug(f"No need to update {obj_id}.") return obj = await storage.get_object_type(obj_id) try: bases = otu.base_from_source(obj.source, obj_id) if not bases: logger.debug( f"{obj_id} is definitely not an ObjectType (subclass of {object.__name__}), maybe mixin?" ) return if bases[0] not in object_types.keys() | built_in_types_names(): logger.debug(f"Getting base class {bases[0]} for {obj_id}.") await get_object_data(object_types, bases[0]) for mixin in bases[1:]: mixin_obj = await storage.get_object_type(mixin) await hlp.run_in_executor( hlp.save_and_import_type_def, mixin_obj.source, mixin_obj.id, object, settings.OBJECT_TYPE_PATH, settings.OBJECT_TYPE_MODULE, ) except Arcor2Exception as e: logger.error( f"Disabling ObjectType {obj.id}: can't get a base. {str(e)}") object_types[obj_id] = ObjectTypeData( ObjectTypeMeta(obj_id, "ObjectType disabled.", disabled=True, problem="Can't get base.", modified=obj.modified)) return logger.debug(f"Updating {obj_id}.") try: type_def = await hlp.run_in_executor( hlp.save_and_import_type_def, obj.source, obj.id, Generic, settings.OBJECT_TYPE_PATH, settings.OBJECT_TYPE_MODULE, ) except Arcor2Exception as e: logger.debug(f"{obj.id} is probably not an ObjectType. {str(e)}") return assert issubclass(type_def, Generic) try: meta = meta_from_def(type_def) except Arcor2Exception as e: logger.error(f"Disabling ObjectType {obj.id}.") logger.debug(e, exc_info=True) object_types[obj_id] = ObjectTypeData( ObjectTypeMeta(obj_id, "ObjectType disabled.", disabled=True, problem=str(e), modified=obj.modified)) return meta.modified = obj.modified if obj.model: try: model = await storage.get_model(obj.model.id, obj.model.type) except Arcor2Exception as e: logger.error( f"{obj.model.id}: failed to get collision model of type {obj.model.type}. {str(e)}" ) meta.disabled = True meta.problem = "Can't get collision model." object_types[obj_id] = ObjectTypeData(meta) return if isinstance(model, Mesh) and model.data_id not in await storage.files_ids(): logger.error( f"Disabling {meta.type} as its mesh file {model.data_id} does not exist." ) meta.disabled = True meta.problem = "Mesh file does not exist." object_types[obj_id] = ObjectTypeData(meta) return kwargs = {model.type().value.lower(): model} meta.object_model = ObjectModel(model.type(), **kwargs) # type: ignore ast = parse(obj.source) otd = ObjectTypeData(meta, type_def, object_actions(type_def, ast), ast) object_types[obj_id] = otd
async def get_object_types() -> UpdatedObjectTypes: """Serves to initialize or update knowledge about awailable ObjectTypes. :return: """ initialization = False # initialize with built-in types, this has to be done just once if not glob.OBJECT_TYPES: logger.debug("Initialization of ObjectTypes.") initialization = True await hlp.run_in_executor(prepare_object_types_dir, settings.OBJECT_TYPE_PATH, settings.OBJECT_TYPE_MODULE) glob.OBJECT_TYPES.update(built_in_types_data()) updated_object_types: ObjectTypeDict = {} object_type_ids: Union[set[str], list[str]] = await storage.get_object_type_ids() if __debug__: # this should uncover potential problems with order in which ObjectTypes are processed import random object_type_ids = list(object_type_ids) random.shuffle(object_type_ids) for obj_id in object_type_ids: await get_object_data(updated_object_types, obj_id) removed_object_ids = { obj for obj in glob.OBJECT_TYPES.keys() if obj not in object_type_ids } - built_in_types_names() updated_object_ids = { k for k in updated_object_types.keys() if k in glob.OBJECT_TYPES } new_object_ids = { k for k in updated_object_types.keys() if k not in glob.OBJECT_TYPES } logger.debug(f"Removed ids: {removed_object_ids}") logger.debug(f"Updated ids: {updated_object_ids}") logger.debug(f"New ids: {new_object_ids}") if not initialization and removed_object_ids: # TODO remove it from sys.modules remove_evt = ChangedObjectTypes([ v.meta for k, v in glob.OBJECT_TYPES.items() if k in removed_object_ids ]) remove_evt.change_type = Event.Type.REMOVE asyncio.ensure_future(notif.broadcast_event(remove_evt)) for removed in removed_object_ids: assert removed not in built_in_types_names( ), "Attempt to remove built-in type." del glob.OBJECT_TYPES[removed] await hlp.run_in_executor(remove_object_type, removed) glob.OBJECT_TYPES.update(updated_object_types) logger.debug(f"All known ids: {glob.OBJECT_TYPES.keys()}") for obj_type in updated_object_types.values(): # if description is missing, try to get it from ancestor(s) if not obj_type.meta.description: try: obj_type.meta.description = obj_description_from_base( glob.OBJECT_TYPES, obj_type.meta) except otu.DataError as e: logger.error( f"Failed to get info from base for {obj_type}, error: '{e}'." ) if not obj_type.meta.disabled and not obj_type.meta.built_in: add_ancestor_actions(obj_type.meta.type, glob.OBJECT_TYPES) if not initialization: if updated_object_ids: update_evt = ChangedObjectTypes([ v.meta for k, v in glob.OBJECT_TYPES.items() if k in updated_object_ids ]) update_evt.change_type = Event.Type.UPDATE asyncio.ensure_future(notif.broadcast_event(update_evt)) if new_object_ids: add_evt = ChangedObjectTypes([ v.meta for k, v in glob.OBJECT_TYPES.items() if k in new_object_ids ]) add_evt.change_type = Event.Type.ADD asyncio.ensure_future(notif.broadcast_event(add_evt)) for obj_type in updated_object_types.values(): if obj_type.type_def and issubclass( obj_type.type_def, Robot) and not obj_type.type_def.abstract(): await get_robot_meta(obj_type) # if object does not change but its base has changed, it has to be reloaded for obj_id, obj in glob.OBJECT_TYPES.items(): if obj_id in updated_object_ids: continue if obj.type_def and obj.meta.base in updated_object_ids: logger.debug( f"Re-importing {obj.meta.type} because its base {obj.meta.base} type has changed." ) obj.type_def = await hlp.run_in_executor( hlp.import_type_def, obj.meta.type, Generic, settings.OBJECT_TYPE_PATH, settings.OBJECT_TYPE_MODULE, ) return UpdatedObjectTypes(new_object_ids, updated_object_ids, removed_object_ids)