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 focus_object_done_cb(req: srpc.o.FocusObjectDone.Request,
ui: WsClient) -> None:
obj_id = req.args.id
if obj_id not in FOCUS_OBJECT:
raise Arcor2Exception(
"focusObjectStart/focusObject has to be called first.")
obj_type = glob.OBJECT_TYPES[osa.get_obj_type_name(obj_id)].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(FOCUS_OBJECT[obj_id]) < len(focus_points):
raise Arcor2Exception("Not all points were done.")
assert glob.SCENE
obj = glob.SCENE.object(obj_id)
assert obj.pose
obj_inst = glob.SCENE_OBJECT_INSTANCES[obj_id]
assert isinstance(obj_inst, GenericWithPose)
fp: List[Position] = []
rp: List[Position] = []
for idx, pose in FOCUS_OBJECT[obj_id].items():
fp.append(focus_points[idx].position)
rp.append(pose.position)
mfa = MeshFocusAction(fp, rp)
glob.logger.debug(f"Attempt to focus for object {obj_id}, data: {mfa}")
try:
new_pose = await scene_srv.focus(mfa)
except scene_srv.SceneServiceException as e:
glob.logger.error(f"Focus failed with: {e}, mfa: {mfa}.")
raise Arcor2Exception("Focusing failed.") from e
glob.logger.info(f"Done focusing for {obj_id}.")
clean_up_after_focus(obj_id)
asyncio.ensure_future(update_scene_object_pose(obj, new_pose, obj_inst))
return None
async def update_object_pose_cb(req: srpc.s.UpdateObjectPose.Request,
ui: WsClient) -> None:
can_modify_scene()
assert glob.SCENE
obj = glob.SCENE.object(req.args.object_id)
if not obj.pose:
raise Arcor2Exception("Object without pose.")
if req.dry_run:
return
asyncio.ensure_future(update_scene_object_pose(obj, req.args.pose))
return None
async def update_object_pose_cb(req: srpc.s.UpdateObjectPose.Request,
ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception(ensure_project_closed=True)
can_modify_scene()
obj = scene.object(req.args.object_id)
if not obj.pose:
raise Arcor2Exception("Object without pose.")
await ensure_locked(req.args.object_id, ui)
if req.dry_run:
return
asyncio.ensure_future(update_scene_object_pose(scene, obj, req.args.pose))
return None
async def update_object_pose_cb(req: srpc.s.UpdateObjectPose.Request,
ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception(ensure_project_closed=True)
try:
if scene_started() and await get_robot_instance(req.args.object_id):
raise Arcor2Exception("Robot's pose can be only updated offline.")
except Arcor2Exception:
pass
obj = scene.object(req.args.object_id)
if not obj.pose:
raise Arcor2Exception("Object without pose.")
await ensure_locked(req.args.object_id, ui)
if req.dry_run:
return
asyncio.ensure_future(update_scene_object_pose(scene, obj, req.args.pose))
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:
"""
assert glob.SCENE
ensure_scene_started()
if req.args.id == req.args.robot.robot_id:
raise Arcor2Exception("Robot cannot update its own pose.")
scene_object = glob.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(req.args.robot.robot_id,
req.args.robot.end_effector)
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_object))
return None
async def focus_object_done_cb(req: srpc.o.FocusObjectDone.Request,
ui: WsClient) -> None:
scene = glob.LOCK.scene_or_exception()
if glob.LOCK.project:
raise Arcor2Exception("Project has to be closed first.")
user_name = glob.USERS.user_name(ui)
obj_id = req.args.id
if obj_id not in FOCUS_OBJECT:
raise Arcor2Exception(
"focusObjectStart/focusObject has to be called first.")
await ensure_locked(req.args.id, ui)
obj_type = glob.OBJECT_TYPES[scene.object(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(FOCUS_OBJECT[obj_id]) < len(focus_points):
raise Arcor2Exception("Not all points were done.")
obj = scene.object(obj_id)
assert obj.pose
obj_inst = glob.SCENE_OBJECT_INSTANCES[obj_id]
assert isinstance(obj_inst, GenericWithPose)
fp: List[Position] = []
rp: List[Position] = []
for idx, pose in FOCUS_OBJECT[obj_id].items():
fp.append(focus_points[idx].position)
rp.append(pose.position)
mfa = MeshFocusAction(fp, rp)
glob.logger.debug(f"Attempt to focus for object {obj_id}, data: {mfa}")
try:
new_pose = await scene_srv.focus(mfa)
except scene_srv.SceneServiceException as e:
glob.logger.error(f"Focus failed with: {e}, mfa: {mfa}.")
raise Arcor2Exception("Focusing failed.") from e
finally:
to_unlock: List[str] = []
try:
to_unlock.append(FOCUS_OBJECT_ROBOT[obj_id].robot_id)
except KeyError:
...
await glob.LOCK.read_unlock(to_unlock, user_name)
glob.logger.info(f"Done focusing for {obj_id}.")
clean_up_after_focus(obj_id)
asyncio.ensure_future(
update_scene_object_pose(scene, obj, new_pose, obj_inst, user_name))
return None
