async def rename_object_cb(req: rpc.scene.RenameObjectRequest, ui: WsClient) -> None: assert glob.SCENE target_obj = glob.SCENE.object(req.args.id) for obj_name in glob.SCENE.object_names(): if obj_name == req.args.new_name: raise Arcor2Exception("Object name already exists.") if not hlp.is_valid_identifier(req.args.new_name): raise Arcor2Exception("Object name invalid (should be snake_case).") if req.dry_run: return None target_obj.name = req.args.new_name glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=target_obj))) return None
async def focus_object_done_cb(req: rpc.objects.FocusObjectDoneRequest, 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)] assert obj_type.object_model and 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.") robot_id, end_effector = FOCUS_OBJECT_ROBOT[obj_id].as_tuple() robot_inst = await osa.get_robot_instance(robot_id) assert hasattr(robot_inst, "focus") # mypy does not deal with hasattr assert glob.SCENE obj = glob.SCENE.object(obj_id) 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) await glob.logger.debug( f'Attempt to focus for object {obj_id}, data: {mfa}') try: obj.pose = await hlp.run_in_executor(robot_inst.focus, mfa) # type: ignore except Arcor2Exception as e: await glob.logger.error(f"Focus failed with: {e}, mfa: {mfa}.") raise Arcor2Exception("Focusing failed.") from e await glob.logger.info(f"Done focusing for {obj_id}.") clean_up_after_focus(obj_id) asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=obj))) asyncio.ensure_future(scene_object_pose_updated(glob.SCENE.id, obj.id)) return None
async def remove_from_scene_cb(req: rpc.scene.RemoveFromSceneRequest, ui: WsClient) -> None: assert glob.SCENE if not req.args.force and { proj.name async for proj in projects_using_object(glob.SCENE.id, req.args.id) }: raise Arcor2Exception( "Can't remove object/service that is used in project(s).") if req.dry_run: return None if req.args.id in glob.SCENE_OBJECT_INSTANCES: obj = glob.SCENE.object(req.args.id) glob.SCENE.objects = [ obj for obj in glob.SCENE.objects if obj.id != req.args.id ] obj_inst = glob.SCENE_OBJECT_INSTANCES[req.args.id] await collision(obj_inst, remove=True) del glob.SCENE_OBJECT_INSTANCES[req.args.id] if req.args.id in OBJECTS_WITH_UPDATED_POSE: OBJECTS_WITH_UPDATED_POSE.remove(req.args.id) asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.REMOVE, data=obj))) elif req.args.id in glob.SERVICES_INSTANCES: # first check if some object is not using it for obj in glob.SCENE.objects: if req.args.id in glob.OBJECT_TYPES[obj.type].needs_services: raise Arcor2Exception( f"Object {obj.id} ({obj.type}) " f"relies on the service to be removed: {req.args.id}.") srv = glob.SCENE.service(req.args.id) glob.SCENE.services = [ srv for srv in glob.SCENE.services if srv.type != req.args.id ] del glob.SERVICES_INSTANCES[req.args.id] asyncio.ensure_future( notif.broadcast_event( events.SceneServiceChanged(events.EventType.REMOVE, data=srv))) else: raise Arcor2Exception("Unknown id.") glob.SCENE.update_modified() asyncio.ensure_future(remove_object_references_from_projects(req.args.id)) return None
async def add_object_to_scene_cb(req: rpc.scene.AddObjectToSceneRequest, ui: WsClient) -> None: assert glob.SCENE obj = common.SceneObject(common.uid(), req.args.name, req.args.type, req.args.pose) await add_object_to_scene(obj, dry_run=req.dry_run) if req.dry_run: return None glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.ADD, data=obj))) return None
async def update_object_pose_cb(req: rpc.scene.UpdateObjectPoseRequest, ui: WsClient) -> None: assert glob.SCENE obj = glob.SCENE.object(req.args.object_id) if glob.OBJECT_TYPES[obj.type].needs_services: raise Arcor2Exception("Can't manipulate object created by service.") obj.pose = req.args.pose glob.SCENE_OBJECT_INSTANCES[req.args.object_id].pose = req.args.pose glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=obj))) OBJECTS_WITH_UPDATED_POSE.add(obj.id) return None
async def update_object_pose_using_robot_cb(req: rpc.objects.UpdateObjectPoseUsingRobotRequest, ui: WsClient) -> \ None: """ Updates object's pose using a pose of the robot's end effector. :param req: :return: """ assert glob.SCENE 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) if glob.OBJECT_TYPES[scene_object.type].needs_services: raise Arcor2Exception("Can't manipulate object created by service.") obj_inst = glob.SCENE_OBJECT_INSTANCES[req.args.id] if obj_inst.collision_model: if isinstance(obj_inst.collision_model, object_type.Mesh ) and req.args.pivot != rpc.objects.PivotEnum.MIDDLE: raise Arcor2Exception( "Only middle pivot point is supported for objects with mesh collision model." ) elif req.args.pivot != rpc.objects.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 obj_inst.collision_model: if isinstance(obj_inst.collision_model, object_type.Box): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.size_z / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.size_z / 2 elif isinstance(obj_inst.collision_model, object_type.Cylinder): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.height / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.height / 2 elif isinstance(obj_inst.collision_model, object_type.Sphere): if req.args.pivot == rpc.objects.PivotEnum.TOP: position_delta.z -= obj_inst.collision_model.radius / 2 elif req.args.pivot == rpc.objects.PivotEnum.BOTTOM: position_delta.z += obj_inst.collision_model.radius / 2 position_delta = position_delta.rotated(new_pose.orientation) scene_object.pose.position.x = new_pose.position.x - position_delta.x scene_object.pose.position.y = new_pose.position.y - position_delta.y scene_object.pose.position.z = new_pose.position.z - position_delta.z scene_object.pose.orientation.set_from_quaternion( new_pose.orientation.as_quaternion() * quaternion.quaternion(0, 1, 0, 0)) obj_inst.pose = scene_object.pose glob.SCENE.update_modified() asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.UPDATE, data=scene_object))) OBJECTS_WITH_UPDATED_POSE.add(scene_object.id) return None
async def auto_add_object_to_scene(obj_type_name: str, dry_run: bool = False) -> None: assert glob.SCENE if obj_type_name not in glob.OBJECT_TYPES: raise Arcor2Exception("Unknown object type.") if obj_type_name in otu.built_in_types_names(): raise Arcor2Exception("Does not work for built in types.") obj_meta = glob.OBJECT_TYPES[obj_type_name] if not obj_meta.needs_services: raise Arcor2Exception("Ordinary object.") if obj_meta.abstract: raise Arcor2Exception("Cannot instantiate abstract type.") if not obj_meta.needs_services <= osa.valid_service_types().keys(): raise Arcor2Exception("Some of required services is not available.") if not obj_meta.needs_services <= glob.SERVICES_INSTANCES.keys(): raise Arcor2Exception("Some of required services is not in the scene.") if dry_run: return None obj_type = await storage.get_object_type(obj_type_name) cls = hlp.type_def_from_source(obj_type.source, obj_type.id, Generic) args: List[Service] = [ glob.SERVICES_INSTANCES[srv_name] for srv_name in obj_meta.needs_services ] assert hasattr(cls, otu.SERVICES_METHOD_NAME) for obj_inst in cls.from_services(*args): # type: ignore assert isinstance(obj_inst, Generic) if not hlp.is_valid_identifier(obj_inst.name): # TODO add message to response await glob.logger.warning(f"Object id {obj_inst.id} invalid.") continue if obj_inst.id in glob.SCENE_OBJECT_INSTANCES: await glob.logger.warning( f"Object id {obj_inst.id} already in scene.") continue if obj_inst.name in instances_names(): await glob.logger.warning(f"Duplicate object name {obj_inst.name}." ) continue glob.SCENE_OBJECT_INSTANCES[obj_inst.id] = obj_inst obj = obj_inst.scene_object() glob.SCENE.objects.append(obj) asyncio.ensure_future( notif.broadcast_event( events.SceneObjectChanged(events.EventType.ADD, data=obj))) if obj_meta.object_model: obj_inst.collision_model = obj_meta.object_model.model() await collision(obj_inst, add=True) return None