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