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)
