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 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 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 object_aiming_start_cb(req: srpc.o.ObjectAimingStart.Request,
ui: WsClient) -> None:
"""Starts the aiming process for a selected object (with mesh) and robot.
Only possible when the scene is started/online.
UI have to acquire write locks for object and robot in advance.
:param req:
:param ui:
:return:
"""
scene = glob.LOCK.scene_or_exception()
if glob.LOCK.project:
raise Arcor2Exception("Project has to be closed first.")
ensure_scene_started()
user_name = glob.USERS.user_name(ui)
if user_name in _objects_being_aimed:
raise Arcor2Exception("Aiming already started.")
obj_id = req.args.object_id
scene_obj = scene.object(obj_id)
obj_type = glob.OBJECT_TYPES[scene_obj.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.")
await ensure_write_locked(req.args.object_id, user_name)
await ensure_write_locked(req.args.robot.robot_id, user_name)
await check_eef_arm(get_robot_instance(req.args.robot.robot_id),
req.args.robot.arm_id, req.args.robot.end_effector)
if req.dry_run:
return
_objects_being_aimed[user_name] = AimedObject(req.args.object_id,
req.args.robot)
logger.info(
f"{user_name} just started aiming of {scene_obj.name} using {scene.object(req.args.robot.robot_id).name}."
)
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
async def object_aiming_prune() -> None:
"""Deletes records for users that already lost their locks.
:return:
"""
to_delete: list[str] = []
# users in db but not holding a lock for the object should be deleted
for un, fo in _objects_being_aimed.items():
if not await glob.LOCK.is_write_locked(fo.obj_id, un):
logger.info(f"Object aiming cancelled for {un}.")
to_delete.append(un)
for td in to_delete:
_objects_being_aimed.pop(td, None)
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 object_aiming_cancel_cb(req: srpc.o.ObjectAimingCancel.Request,
ui: WsClient) -> None:
"""Cancel aiming of the object.
:param req:
:param ui:
:return:
"""
fo, user_name = await object_aiming_check(ui)
if req.dry_run:
return
_objects_being_aimed.pop(user_name, None)
if glob.LOCK.scene:
logger.info(
f"Aiming for {glob.LOCK.scene.object(fo.obj_id).name} cancelled by {user_name}."
)
async def run_temp_package(package_id: str, start_paused: bool = False, breakpoints: Optional[set[str]] = None) -> None:
# TODO lock scene and project?
assert glob.LOCK.scene
assert glob.LOCK.project
project_id = glob.LOCK.project.id
glob.TEMPORARY_PACKAGE = True
scene_online = scene_started()
if scene_online:
await stop_scene(glob.LOCK.scene) # the package will start it on its own
await project.close_project(show_mainscreen_after_that=False)
req = erpc.RunPackage.Request
exe_req = req(get_id(), args=req.Args(package_id, start_paused, breakpoints))
exe_resp = await manager_request(exe_req)
if not exe_resp.result:
logger.warning(f"Execution of temporary package failed with: {exe_resp.messages}.")
else:
await server_events.package_started.wait()
await server_events.package_stopped.wait()
logger.info("Temporary package stopped, let's remove it and reopen project.")
glob.TEMPORARY_PACKAGE = False
await manager_request(erpc.DeletePackage.Request(get_id(), args=rpc.common.IdArgs(package_id)))
await project.open_project(project_id)
assert glob.LOCK.scene
assert glob.LOCK.project
await notif.broadcast_event(
sevts.p.OpenProject(sevts.p.OpenProject.Data(glob.LOCK.scene.scene, glob.LOCK.project.project))
)
if scene_online:
await start_scene(glob.LOCK.scene)
async def remove_object_references_from_projects(obj_id: str) -> None:
assert glob.LOCK.scene
updated_project_ids: set[str] = set()
async for project in projects_using_object_as_parent(
glob.LOCK.scene.id, obj_id):
# action_ids: set[str] = set()
# delete actions using the object
for action_to_delete in {
act.id
for act in project.actions if act.parse_type()[0] == obj_id
}:
project.remove_action(action_to_delete)
# delete actions using obj's action points as parameters
# TODO fix this!
"""
actions_using_invalid_param: set[str] = \
{act.id for act in ap.actions for param in act.parameters
if param.type in (ActionParameterTypeEnum.JOINTS, ActionParameterTypeEnum.POSE) and
param.value.startswith(obj_id)}
ap.actions = [act for act in ap.actions if act.id not in actions_using_invalid_param]
# get IDs of remaining actions
action_ids.update({act.id for act in ap.actions})
"""
# valid_ids: set[str] = action_ids | ActionIOEnum.set()
# TODO remove invalid logic items
await storage.update_project(project)
updated_project_ids.add(project.id)
logger.info("Updated projects: {}".format(updated_project_ids))
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 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.")
