async def get_robot_arms_cb(req: srpc.r.GetRobotArms.Request, ui: WsClient) -> srpc.r.GetRobotArms.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
return srpc.r.GetRobotArms.Response(data=await robot.get_arms(get_robot_instance(req.args.robot_id)))
async def register_for_robot_event_cb(req: srpc.r.RegisterForRobotEvent.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
# check if robot exists
robot_inst = get_robot_instance(req.args.robot_id)
if req.args.what == req.args.RegisterEnum.JOINTS:
await register(
req, robot_inst, ui, ROBOT_JOINTS_TASKS, glob.ROBOT_JOINTS_REGISTERED_UIS, robot_joints_event
)
elif req.args.what == req.args.RegisterEnum.EEF_POSE:
if isinstance(robot_inst, MultiArmRobot):
for arm_id in await robot.get_arms(robot_inst):
if await robot.get_end_effectors(robot_inst, arm_id):
break
else:
raise Arcor2Exception("Robot does not have any end effector.")
elif not await robot.get_end_effectors(robot_inst, None):
raise Arcor2Exception("Robot does not have any end effector.")
await register(req, robot_inst, ui, EEF_POSE_TASKS, glob.ROBOT_EEF_REGISTERED_UIS, robot_eef_pose_event)
else:
raise Arcor2Exception(f"Option '{req.args.what.value}' not implemented.")
return None
async def projects_with_scene_cb(
req: srpc.s.ProjectsWithScene.Request,
ui: WsClient) -> srpc.s.ProjectsWithScene.Response:
async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)):
resp = srpc.s.ProjectsWithScene.Response()
resp.data = await associated_projects(req.args.id)
return resp
async def get_end_effector_pose_cb(
req: srpc.r.GetEndEffectorPose.Request,
ui: WsClient) -> srpc.r.GetEndEffectorPose.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.robot_id, glob.USERS.user_name(ui)):
ensure_scene_started()
return srpc.r.GetEndEffectorPose.Response(
data=await robot.get_end_effector_pose(
await osa.get_robot_instance(req.args.robot_id),
req.args.end_effector_id, req.args.arm_id))
async def fk_cb(req: srpc.r.ForwardKinematics.Request, ui: WsClient) -> srpc.r.ForwardKinematics.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.forward_kinematics.__name__)
pose = await robot.fk(robot_inst, req.args.end_effector_id, req.args.arm_id, req.args.joints)
resp = srpc.r.ForwardKinematics.Response()
resp.data = pose
return resp
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 ik_cb(req: srpc.r.InverseKinematics.Request, ui: WsClient) -> srpc.r.InverseKinematics.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock([req.args.robot_id, req.args.end_effector_id], glob.USERS.user_name(ui)):
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.inverse_kinematics.__name__)
joints = await robot.ik(
robot_inst, req.args.end_effector_id, req.args.pose, req.args.start_joints, req.args.avoid_collisions
)
resp = srpc.r.InverseKinematics.Response()
resp.data = joints
return resp
async def focus_object_cb(req: srpc.o.FocusObject.Request,
ui: WsClient) -> srpc.o.FocusObject.Response:
async with ctx_read_lock(req.args.object_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.")
obj_id = req.args.object_id
pt_idx = req.args.point_idx
if obj_id not in glob.SCENE_OBJECT_INSTANCES:
raise Arcor2Exception("Unknown object_id.")
obj_type = glob.OBJECT_TYPES[scene.object(obj_id).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.")
if obj_id not in FOCUS_OBJECT:
glob.logger.info(f"Start of focusing for {obj_id}.")
FOCUS_OBJECT[obj_id] = {}
robot_id, end_effector, arm_id = FOCUS_OBJECT_ROBOT[obj_id].as_tuple()
FOCUS_OBJECT[obj_id][pt_idx] = await get_end_effector_pose(
await osa.get_robot_instance(robot_id), end_effector, arm_id)
r = srpc.o.FocusObject.Response()
r.data = r.Data(finished_indexes=list(FOCUS_OBJECT[obj_id].keys()))
return r
async def action_param_values_cb(
req: srpc.o.ActionParamValues.Request,
ui: WsClient) -> srpc.o.ActionParamValues.Response:
glob.LOCK.scene_or_exception()
async with ctx_read_lock(req.args.id, glob.USERS.user_name(ui)):
ensure_scene_started()
inst = get_instance(req.args.id)
parent_params = {}
for pp in req.args.parent_params:
parent_params[pp.id] = pp.value
try:
method_name, required_parent_params = inst.DYNAMIC_PARAMS[
req.args.param_id]
except KeyError:
raise Arcor2Exception(
"Unknown parameter or values not constrained.")
if parent_params.keys() != required_parent_params:
raise Arcor2Exception("Not all required parent params were given.")
# TODO validate method parameters vs parent_params (check types)?
resp = srpc.o.ActionParamValues.Response()
try:
method = getattr(inst, method_name)
except AttributeError:
glob.logger.error(
f"Unable to get values for parameter {req.args.param_id}, "
f"object/service {inst.id} has no method named {method_name}.")
raise Arcor2Exception("System error.")
# TODO update hlp.run_in_executor to support kwargs
resp.data = await asyncio.get_event_loop().run_in_executor(
None, functools.partial(method, **parent_params))
return resp
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
async def test_ctx_read_lock() -> None:
test = "test"
user = "******"
glob.LOCK = Lock({})
assert await glob.LOCK.get_locked_roots_count() == 0
glob.LOCK.scene = UpdateableCachedScene(cmn.Scene(test, description=test))
glob.LOCK.project = UpdateableCachedProject(cmn.Project(test, glob.LOCK.scene.id, description=test, has_logic=True))
async def patch() -> set[str]:
return {glob.LOCK.project_or_exception().id, glob.LOCK.scene_or_exception().id}
storage.get_project_ids = storage.get_scene_ids = patch
# add some scene and project objects
test_object = cmn.SceneObject(test, "TestType")
glob.LOCK.scene.upsert_object(test_object)
ap = glob.LOCK.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap", cmn.Position(0, 0, 0), test_object.id)
ap_ap = glob.LOCK.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap", cmn.Position(0, 0, 1), ap.id)
assert await glob.LOCK.get_locked_roots_count() == 0
await glob.LOCK.write_lock(ap_ap.id, user, True)
assert await glob.LOCK.is_write_locked(test_object.id, user)
assert await glob.LOCK.is_write_locked(ap.id, user)
assert await glob.LOCK.is_write_locked(ap_ap.id, user)
async with ctx_read_lock(test_object.id, user):
pass
assert await glob.LOCK.is_write_locked(test_object.id, user)
assert await glob.LOCK.is_write_locked(ap.id, user)
assert await glob.LOCK.is_write_locked(ap_ap.id, user)
