async def move_to_joints_cb(req: srpc.r.MoveToJoints.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 check_feature(robot_inst, Robot.move_to_joints.__name__)
await robot.check_robot_before_move(robot_inst)
asyncio.ensure_future(
robot.move_to_joints(robot_inst, req.args.joints, req.args.speed,
req.args.safe, user_name, req.args.arm_id))
async def move_to_action_point_cb(req: srpc.r.MoveToActionPoint.Request, ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception()
project = glob.LOCK.project_or_exception()
async with ctx_write_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await robot.check_robot_before_move(robot_inst)
if (req.args.orientation_id is None) == (req.args.joints_id is None):
raise Arcor2Exception("Set orientation or joints. Not both.")
if req.args.orientation_id:
await check_feature(robot_inst, Robot.move_to_pose.__name__)
if req.args.end_effector_id is None:
raise Arcor2Exception("eef id has to be set.")
pose = tr.abs_pose_from_ap_orientation(scene, project, req.args.orientation_id)
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_orientation(
robot_inst,
req.args.end_effector_id,
pose,
req.args.speed,
req.args.orientation_id,
req.args.safe,
)
)
elif req.args.joints_id:
await check_feature(robot_inst, Robot.move_to_joints.__name__)
joints = project.joints(req.args.joints_id)
# TODO check if the joints are within limits and reachable (dry_run)
asyncio.ensure_future(
robot.move_to_ap_joints(robot_inst, joints.joints, req.args.speed, req.args.joints_id, req.args.safe)
)
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 remove_from_scene_cb(req: srpc.s.RemoveFromScene.Request,
ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception(ensure_project_closed=True)
user_name = glob.USERS.user_name(ui)
to_lock = await get_unlocked_objects(req.args.id, user_name)
async with ctx_write_lock(to_lock, user_name, auto_unlock=req.dry_run):
can_modify_scene()
if not req.args.force and {
proj.name
async for proj in projects_using_object(scene.id, req.args.id)
}:
raise Arcor2Exception(
"Can't remove object that is used in project(s).")
if req.dry_run:
return None
if req.args.id not in scene.object_ids:
raise Arcor2Exception("Unknown id.")
await glob.LOCK.write_unlock(req.args.id, user_name)
obj = scene.object(req.args.id)
scene.delete_object(req.args.id)
if req.args.id in glob.OBJECTS_WITH_UPDATED_POSE:
glob.OBJECTS_WITH_UPDATED_POSE.remove(req.args.id)
evt = sevts.s.SceneObjectChanged(obj)
evt.change_type = Event.Type.REMOVE
asyncio.ensure_future(notif.broadcast_event(evt))
# TODO this should be done after scene is saved
asyncio.ensure_future(
remove_object_references_from_projects(req.args.id))
return None
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 start_scene(scene: CachedScene) -> None:
"""Creates instances of scene objects."""
async def _start_scene() -> bool:
glob.logger.info(f"Starting the {scene.name} scene.")
await set_scene_state(SceneState.Data.StateEnum.Starting)
try:
await scene_srv.stop()
except Arcor2Exception:
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
prio_dict: DefaultDict[int, List[SceneObject]] = defaultdict(list)
for obj in scene.objects:
type_def = glob.OBJECT_TYPES[obj.type].type_def
assert type_def
prio_dict[type_def.INIT_PRIORITY].append(obj)
for prio in sorted(prio_dict.keys(), reverse=True):
assert prio_dict[prio]
# object initialization could take some time - let's do it in parallel (grouped by priority)
tasks = [
asyncio.ensure_future(
create_object_instance(
obj, object_overrides[obj.id]
if obj.id in object_overrides else None))
for obj in prio_dict[prio]
]
try:
await asyncio.gather(*tasks)
except Arcor2Exception as e:
for t in tasks:
t.cancel()
glob.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:
glob.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_NAME]
if glob.LOCK.project:
to_lock.append(glob.LOCK.SpecialValues.PROJECT_NAME)
try:
async with ctx_write_lock(to_lock,
glob.LOCK.SpecialValues.SERVER_NAME):
ret = await _start_scene()
except Arcor2Exception as e:
glob.logger.error(f"Failed to start the scene. {str(e)}")
return
assert ret == scene_started()
assert ret == await scene_srv.started()
glob.logger.info("Scene started.")
async def new_object_type_cb(req: srpc.o.NewObjectType.Request,
ui: WsClient) -> None:
async with ctx_write_lock(glob.LOCK.SpecialValues.ADDING_OBJECT, ui):
meta = req.args
if meta.type in glob.OBJECT_TYPES:
raise Arcor2Exception("Object type already exists.")
hlp.is_valid_type(meta.type)
if meta.base not in glob.OBJECT_TYPES:
raise Arcor2Exception(
f"Unknown base object type '{meta.base}', "
f"known types are: {', '.join(glob.OBJECT_TYPES.keys())}.")
base = glob.OBJECT_TYPES[meta.base]
if base.meta.disabled:
raise Arcor2Exception("Base object is disabled.")
assert base.type_def is not None
if issubclass(base.type_def, Robot):
raise Arcor2Exception("Can't subclass Robot.")
meta.has_pose = issubclass(base.type_def, GenericWithPose)
if not meta.has_pose and meta.object_model:
raise Arcor2Exception(
"Object without pose can't have collision model.")
if req.dry_run:
return None
obj = meta.to_object_type()
ast = new_object_type(glob.OBJECT_TYPES[meta.base].meta, meta)
obj.source = tree_to_str(ast)
if meta.object_model:
if meta.object_model.type == Model3dType.MESH:
# TODO check whether mesh id exists - if so, then use existing mesh, if not, upload a new one
# ...get whole mesh (focus_points) based on mesh id
assert meta.object_model.mesh
try:
meta.object_model.mesh = await storage.get_mesh(
meta.object_model.mesh.id)
except storage.ProjectServiceException as e:
glob.logger.error(e)
raise Arcor2Exception(
f"Mesh ID {meta.object_model.mesh.id} does not exist.")
else:
meta.object_model.model().id = meta.type
await storage.put_model(meta.object_model.model())
type_def = await hlp.run_in_executor(
hlp.save_and_import_type_def,
obj.source,
obj.id,
base.type_def,
settings.OBJECT_TYPE_PATH,
settings.OBJECT_TYPE_MODULE,
)
assert issubclass(type_def, base.type_def)
actions = object_actions(type_def, ast)
await storage.update_object_type(obj)
glob.OBJECT_TYPES[meta.type] = ObjectTypeData(meta, type_def, actions,
ast)
add_ancestor_actions(meta.type, glob.OBJECT_TYPES)
evt = sevts.o.ChangedObjectTypes([meta])
evt.change_type = events.Event.Type.ADD
asyncio.ensure_future(notif.broadcast_event(evt))
return None
async def update_object_pose_using_robot_cb(
req: srpc.o.UpdateObjectPoseUsingRobot.Request, ui: WsClient) -> None:
"""Updates object's pose using a pose of the robot's end effector.
:param req:
:return:
"""
if req.args.id == req.args.robot.robot_id:
raise Arcor2Exception("Robot cannot update its own pose.")
scene = glob.LOCK.scene_or_exception(ensure_project_closed=True)
user_name = glob.USERS.user_name(ui)
to_lock = await get_unlocked_objects(
[obj for obj in (req.args.robot.robot_id, req.args.id)], user_name)
async with ctx_write_lock(to_lock, user_name):
ensure_scene_started()
robot_inst = await get_robot_instance(req.args.robot.robot_id)
await check_eef_arm(robot_inst, req.args.robot.arm_id,
req.args.robot.end_effector)
scene_object = scene.object(req.args.id)
obj_type = glob.OBJECT_TYPES[scene_object.type]
if not obj_type.meta.has_pose:
raise Arcor2Exception("Object without pose.")
object_model = obj_type.meta.object_model
if object_model:
collision_model = object_model.model()
if isinstance(collision_model, object_type.Mesh
) and req.args.pivot != req.args.PivotEnum.MIDDLE:
raise Arcor2Exception(
"Only middle pivot point is supported for objects with mesh collision model."
)
elif req.args.pivot != req.args.PivotEnum.MIDDLE:
raise Arcor2Exception(
"Only middle pivot point is supported for objects without collision model."
)
new_pose = await get_end_effector_pose(robot_inst,
req.args.robot.end_effector,
req.args.robot.arm_id)
position_delta = common.Position()
if object_model:
collision_model = object_model.model()
if isinstance(collision_model, object_type.Box):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.size_z / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.size_z / 2
elif isinstance(collision_model, object_type.Cylinder):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.height / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.height / 2
elif isinstance(collision_model, object_type.Sphere):
if req.args.pivot == req.args.PivotEnum.TOP:
position_delta.z -= collision_model.radius / 2
elif req.args.pivot == req.args.PivotEnum.BOTTOM:
position_delta.z += collision_model.radius / 2
position_delta = position_delta.rotated(new_pose.orientation)
assert scene_object.pose
scene_object.pose.position = new_pose.position - position_delta
scene_object.pose.orientation.set_from_quaternion(
new_pose.orientation.as_quaternion() *
quaternion.quaternion(0, 1, 0, 0))
asyncio.ensure_future(update_scene_object_pose(scene, scene_object))
return None
async def delete_object_type_cb(
req: srpc.o.DeleteObjectTypes.Request,
ui: WsClient) -> srpc.o.DeleteObjectTypes.Response:
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 _delete_ot(ot: str) -> None:
obj_type = glob.OBJECT_TYPES[ot]
if obj_type.meta.built_in:
raise Arcor2Exception("Can't delete built-in type.")
for obj in glob.OBJECT_TYPES.values():
if obj.meta.base == ot:
raise Arcor2Exception(
f"Object type is base of '{obj.meta.type}'.")
async for scene in scenes():
check_scene_for_object_type(scene, ot)
if glob.LOCK.scene:
check_scene_for_object_type(glob.LOCK.scene, ot)
if req.dry_run:
return
await asyncio.gather(storage.delete_object_type(ot),
_delete_model(obj_type), remove_object_type(ot))
await glob.LOCK.write_unlock(
ot, user_name
) # need to be unlocked while it exists in glob.OBJECT_TYPES
del glob.OBJECT_TYPES[ot]
evt = sevts.o.ChangedObjectTypes([obj_type.meta])
evt.change_type = events.Event.Type.REMOVE
asyncio.create_task(notif.broadcast_event(evt))
user_name = glob.USERS.user_name(ui)
obj_types_to_delete: list[str] = (list(
req.args) if req.args is not None else [
obj.meta.type for obj in glob.OBJECT_TYPES.values()
if not obj.meta.built_in
])
response = srpc.o.DeleteObjectTypes.Response()
response.data = []
async with ctx_write_lock(obj_types_to_delete,
user_name,
auto_unlock=False,
dry_run=req.dry_run):
res = await asyncio.gather(
*[_delete_ot(ot) for ot in obj_types_to_delete],
return_exceptions=True)
for idx, r in enumerate(res):
if isinstance(r, Arcor2Exception):
response.data.append(
srpc.o.DeleteObjectTypes.Response.Data(
obj_types_to_delete[idx], str(r)))
else:
assert r is None
if not response.data:
response.data = None
if response.data:
response.result = False
response.messages = []
response.messages.append("Failed to delete one or more ObjectTypes.")
return response
async def new_object_type_cb(req: srpc.o.NewObjectType.Request,
ui: WsClient) -> None:
async with ctx_write_lock(glob.LOCK.SpecialValues.ADDING_OBJECT,
glob.USERS.user_name(ui)):
meta = req.args
if meta.type in glob.OBJECT_TYPES:
raise Arcor2Exception("Object type already exists.")
hlp.is_valid_type(meta.type)
if meta.base not in glob.OBJECT_TYPES:
raise Arcor2Exception(
f"Unknown base object type '{meta.base}', "
f"known types are: {', '.join(glob.OBJECT_TYPES.keys())}.")
base = glob.OBJECT_TYPES[meta.base]
if base.meta.disabled:
raise Arcor2Exception("Base object is disabled.")
assert base.type_def is not None
if issubclass(base.type_def, Robot):
raise Arcor2Exception("Can't subclass Robot.")
meta.has_pose = issubclass(base.type_def, GenericWithPose)
if issubclass(base.type_def, CollisionObject):
if not meta.object_model:
raise Arcor2Exception(
"Objects based on CollisionObject must have collision model."
)
else:
if meta.object_model:
raise Arcor2Exception(
"Only objects based on CollisionObject can have collision model."
)
if req.dry_run:
return None
obj = meta.to_object_type()
ast = new_object_type(glob.OBJECT_TYPES[meta.base].meta, meta)
obj.source = tree_to_str(ast)
if meta.object_model:
await update_object_model(meta, meta.object_model)
type_def = await hlp.run_in_executor(
hlp.save_and_import_type_def,
obj.source,
obj.id,
base.type_def,
settings.OBJECT_TYPE_PATH,
settings.OBJECT_TYPE_MODULE,
)
assert issubclass(type_def, base.type_def)
actions = object_actions(type_def, ast)
meta.modified = await storage.update_object_type(obj)
glob.OBJECT_TYPES[meta.type] = ObjectTypeData(meta, type_def, actions,
ast)
add_ancestor_actions(meta.type, glob.OBJECT_TYPES)
evt = sevts.o.ChangedObjectTypes([meta])
evt.change_type = events.Event.Type.ADD
asyncio.ensure_future(notif.broadcast_event(evt))
return None