async def calibrate_robot_cb(req: srpc.r.CalibrateRobot.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
user_name = glob.USERS.user_name(ui)
ensure_scene_started()
if not req.args.camera_id:
for obj in glob.SCENE_OBJECT_INSTANCES.values():
if isinstance(obj, Camera):
req.args.camera_id = obj.id
break
else:
raise Arcor2Exception("No camera found.")
async with ctx_write_lock(req.args.camera_id, user_name, auto_unlock=False):
robot_inst = get_robot_instance(req.args.robot_id)
if not robot_inst.urdf_package_name:
raise Arcor2Exception("Robot with model required!")
camera_inst = get_instance(req.args.camera_id, Camera)
if camera_inst.color_camera_params is None:
raise Arcor2Exception("Calibrated camera required!")
await ensure_write_locked(req.args.robot_id, user_name)
asyncio.ensure_future(calibrate_robot(robot_inst, camera_inst, req.args.move_to_calibration_pose, user_name))
return None
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
def get_camera_instance(obj_id: str) -> Camera:
obj = get_instance(obj_id)
if not isinstance(obj, Camera):
raise Arcor2Exception("Not a camera.")
return obj
async def cancel_action_cb(req: srpc.p.CancelAction.Request,
ui: WsClient) -> None:
assert glob.SCENE
assert glob.PROJECT
if not glob.RUNNING_ACTION:
raise Arcor2Exception("No action is running.")
action = glob.PROJECT.action(glob.RUNNING_ACTION)
obj_id, action_name = action.parse_type()
obj = get_instance(obj_id)
if not find_object_action(glob.SCENE, action).meta.cancellable:
raise Arcor2Exception("Action is not cancellable.")
try:
action_method = getattr(obj, action_name)
cancel_method = getattr(obj,
obj.CANCEL_MAPPING[action_method.__name__])
except AttributeError as e:
raise Arcor2Exception("Internal error.") from e
cancel_params: Dict[str, Any] = {}
# TODO fix or remove this?
"""
cancel_sig = inspect.signature(cancel_method)
assert glob.RUNNING_ACTION_PARAMS is not None
for param_name in cancel_sig.parameters.keys():
try:
cancel_params[param_name] = glob.RUNNING_ACTION_PARAMS[param_name]
except KeyError as e:
raise Arcor2Exception("Cancel method parameters should be subset of action parameters.") from e
"""
await hlp.run_in_executor(cancel_method, *cancel_params.values())
asyncio.ensure_future(notif.broadcast_event(sevts.a.ActionCancelled()))
glob.RUNNING_ACTION = None
glob.RUNNING_ACTION_PARAMS = None
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 execute_action_cb(req: srpc.p.ExecuteAction.Request,
ui: WsClient) -> None:
assert glob.SCENE
assert glob.PROJECT
ensure_scene_started()
if glob.RUNNING_ACTION:
raise Arcor2Exception(
f"Action {glob.RUNNING_ACTION} is being executed. "
f"Only one action can be executed at a time.")
action = glob.PROJECT.action(req.args.action_id)
obj_id, action_name = action.parse_type()
params: List[Any] = []
for param in action.parameters:
if param.type == common.ActionParameter.TypeEnum.LINK:
parsed_link = param.parse_link()
try:
results = project.PREV_RESULTS[parsed_link.action_id]
except KeyError:
prev_action = glob.PROJECT.action(parsed_link.action_id)
raise Arcor2Exception(
f"Action '{prev_action.name}' has to be executed first.")
# an action result could be a tuple or a single value
if isinstance(results, tuple):
params.append(results[parsed_link.output_index])
else:
assert parsed_link.output_index == 0
params.append(results)
elif param.type == common.ActionParameter.TypeEnum.CONSTANT:
const = glob.PROJECT.constant(param.str_from_value())
# TODO use plugin to get the value
import json
params.append(json.loads(const.value))
else:
try:
params.append(
plugin_from_type_name(
param.type).parameter_execution_value(
get_types_dict(), glob.SCENE, glob.PROJECT,
action.id, param.name))
except ParameterPluginException as e:
glob.logger.error(e)
raise Arcor2Exception(
f"Failed to get value for parameter {param.name}.")
obj = get_instance(obj_id)
if isinstance(obj, Robot):
if obj.move_in_progress:
raise Arcor2Exception(
"Can't execute actions while the robot moves.")
if not hasattr(obj, action_name):
raise Arcor2Exception(
"Internal error: object does not have the requested method.")
if req.dry_run:
return
glob.RUNNING_ACTION = action.id
glob.RUNNING_ACTION_PARAMS = params
glob.logger.debug(
f"Running action {action.name} ({type(obj)}/{action_name}), params: {params}."
)
# schedule execution and return success
asyncio.ensure_future(
project.execute_action(getattr(obj, action_name), params))
return None