def lock() -> Lock:
"""Creates lock with initialized scene and project."""
test = "test"
lock = Lock({})
scene = UpdateableCachedScene(cmn.Scene(test, description=test))
lock.scene = scene
project = UpdateableCachedProject(cmn.Project(test, lock.scene.id, description=test, has_logic=True))
lock.project = project
assert lock.scene == scene
assert lock.scene_or_exception() == scene
assert lock.project == project
assert lock.project_or_exception() == project
# add some scene and project objects
test_object = cmn.SceneObject(test, "TestType")
lock.scene.upsert_object(test_object)
ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap", cmn.Position(0, 0, 0))
ap_ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap", cmn.Position(0, 0, 1), ap.id)
ap_ap_ap = lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap_ap_ap", cmn.Position(0, 0, 2), ap_ap.id)
lock.project.upsert_action_point(cmn.BareActionPoint.uid(), "ap2", cmn.Position(0, 1, 0))
ori = cmn.NamedOrientation("ori", cmn.Orientation())
lock.project.upsert_orientation(ap_ap_ap.id, ori)
action = cmn.Action("action", "test/type", parameters=[], flows=[])
lock.project.upsert_action(ap_ap_ap.id, action)
return lock
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)
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
def test_run_simple_project(start_processes: None, ars: ARServer) -> None:
event(ars, events.c.ShowMainScreen)
assert ars.call_rpc(
rpc.s.NewScene.Request(get_id(),
rpc.s.NewScene.Request.Args("Test scene")),
rpc.s.NewScene.Response).result
scene_data = event(ars, events.s.OpenScene).data
assert scene_data
scene = scene_data.scene
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
get_id(),
rpc.s.AddObjectToScene.Request.Args("time_actions",
TimeActions.__name__)),
rpc.s.AddObjectToScene.Response,
).result
obj = event(ars, events.s.SceneObjectChanged).data
assert obj
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
get_id(),
rpc.s.AddObjectToScene.Request.Args("random_actions",
RandomActions.__name__)),
rpc.s.AddObjectToScene.Response,
).result
obj2 = event(ars, events.s.SceneObjectChanged).data
assert obj2
# ------------------------------------------------------------------------------------------------------------------
assert ars.call_rpc(
rpc.p.NewProject.Request(
get_id(), rpc.p.NewProject.Request.Args(scene.id, "Project name")),
rpc.p.NewProject.Response,
).result
event(ars, events.s.SceneSaved)
proj = event(ars, events.p.OpenProject).data
assert proj
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
get_id(),
rpc.p.AddActionPoint.Request.Args("ap1", common.Position())),
rpc.p.AddActionPoint.Response,
).result
ap = event(ars, events.p.ActionPointChanged).data
assert ap is not None
assert ars.call_rpc(
rpc.p.AddProjectParameter.Request(
get_id(),
rpc.p.AddProjectParameter.Request.Args("min_time", "double",
json.dumps(0.45))),
rpc.p.AddActionPoint.Response,
).result
c1 = event(ars, events.p.ProjectParameterChanged).data
assert c1
assert ars.call_rpc(
rpc.p.AddAction.Request(
get_id(),
rpc.p.AddAction.Request.Args(
ap.id,
"test_action",
f"{obj2.id}/{RandomActions.random_double.__name__}",
[
common.ActionParameter(
"range_min",
common.ActionParameter.TypeEnum.PROJECT_PARAMETER,
json.dumps(c1.id)),
common.ActionParameter("range_max", "double", "0.55"),
],
[common.Flow(outputs=["random_value"])],
),
),
rpc.p.AddAction.Response,
).result
action = event(ars, events.p.ActionChanged).data
assert action is not None
assert ars.call_rpc(
rpc.p.AddAction.Request(
get_id(),
rpc.p.AddAction.Request.Args(
ap.id,
"test_action2",
f"{obj.id}/{TimeActions.sleep.__name__}",
[
common.ActionParameter(
"seconds",
common.ActionParameter.TypeEnum.LINK,
json.dumps(
f"{action.id}/{common.FlowTypes.DEFAULT}/0"),
)
],
[common.Flow()],
),
),
rpc.p.AddAction.Response,
).result
action2 = event(ars, events.p.ActionChanged).data
assert action2 is not None
add_logic_item(ars, common.LogicItem.START, action.id)
event(ars, events.lk.ObjectsUnlocked)
add_logic_item(ars, action.id, action2.id)
event(ars, events.lk.ObjectsUnlocked)
add_logic_item(ars, action2.id, common.LogicItem.END)
event(ars, events.lk.ObjectsUnlocked)
save_project(ars)
LOGGER.debug(project_service.get_project(proj.project.id))
# TODO test also temporary package
close_project(ars)
# ------------------------------------------------------------------------------------------------------------------
event(ars, events.c.ShowMainScreen)
assert ars.call_rpc(
rpc.b.BuildProject.Request(
get_id(),
rpc.b.BuildProject.Request.Args(proj.project.id, "Package name")),
rpc.b.BuildProject.Response,
).result
package = event(ars, eevents.PackageChanged).data
assert package is not None
assert ars.call_rpc(
erpc.RunPackage.Request(get_id(),
erpc.RunPackage.Request.Args(package.id)),
erpc.RunPackage.Response).result
ps = event(ars, arcor2_events.PackageState).data
assert ps
assert ps.package_id == package.id
assert ps.state == ps.state.RUNNING
pi = event(ars, arcor2_events.PackageInfo).data
assert pi
assert pi.package_id == package.id
# random_double action
act_state_before = event(ars, arcor2_events.ActionStateBefore).data
assert act_state_before
assert act_state_before.action_id == action.id
assert act_state_before.parameters
assert len(act_state_before.parameters) == 2
act_state_after = event(ars, arcor2_events.ActionStateAfter).data
assert act_state_after
assert act_state_after.action_id == action.id
assert act_state_after.results
assert len(act_state_after.results) == 1
assert isinstance(json.loads(act_state_after.results[0]), float)
# sleep action
act2_state_before = event(ars, arcor2_events.ActionStateBefore).data
assert act2_state_before
assert act2_state_before.action_id == action2.id
assert act2_state_before.parameters
assert len(act2_state_before.parameters) == 1
act2_state_after = event(ars, arcor2_events.ActionStateAfter).data
assert act2_state_after
assert act2_state_after.action_id == action2.id
assert act2_state_after.results is None
# TODO pause, resume
assert ars.call_rpc(erpc.StopPackage.Request(get_id()),
erpc.StopPackage.Response).result
ps2 = wait_for_event(ars, arcor2_events.PackageState).data
assert ps2
assert ps2.package_id == package.id
assert ps2.state == ps.state.STOPPING
ps3 = wait_for_event(ars, arcor2_events.PackageState).data
assert ps3
assert ps3.package_id == package.id
assert ps3.state == ps.state.STOPPED
show_main_screen_event = event(ars, events.c.ShowMainScreen)
assert show_main_screen_event.data
assert show_main_screen_event.data.what == events.c.ShowMainScreen.Data.WhatEnum.PackagesList
assert show_main_screen_event.data.highlight == package.id
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
def project(ars: ARServer, scene: common.Scene) -> common.Project:
"""Creates project with following objects:
ap - global AP
ap_ap - child of ap
ap_ap_ap - child of ap_ap
ori - ap_ap_ap orientation
"""
test = "Test project"
assert ars.call_rpc(
rpc.p.NewProject.Request(
uid(), rpc.p.NewProject.Request.Args(scene.id, test, test)),
rpc.p.NewProject.Response).result
project_evt = event(ars, events.p.OpenProject)
assert project_evt.data
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
uid(),
rpc.p.AddActionPoint.Request.Args("ap", common.Position(0, 0, 0))),
rpc.p.AddActionPoint.Response,
).result
ap_evt = event(ars, events.p.ActionPointChanged)
assert ap_evt.data
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
uid(),
rpc.p.AddActionPoint.Request.Args("ap_ap",
common.Position(0, 0, 1),
ap_evt.data.id)),
rpc.p.AddActionPoint.Response,
).result
ap_ap_evt = event(ars, events.p.ActionPointChanged)
assert ap_ap_evt.data
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
uid(),
rpc.p.AddActionPoint.Request.Args("ap_ap_ap",
common.Position(0, 0, 2),
ap_ap_evt.data.id)),
rpc.p.AddActionPoint.Response,
).result
ap_ap_ap_evt = event(ars, events.p.ActionPointChanged)
assert ap_ap_ap_evt.data
lock_object(ars, ap_ap_ap_evt.data.id)
assert ars.call_rpc(
rpc.p.AddActionPointOrientation.Request(
uid(),
rpc.p.AddActionPointOrientation.Request.Args(
ap_ap_ap_evt.data.id, common.Orientation(), "ori")),
rpc.p.AddActionPointOrientation.Response,
).result
ori_evt = event(ars, events.p.OrientationChanged)
assert ori_evt.data
unlock_object(ars, ap_ap_ap_evt.data.id)
assert ars.call_rpc(rpc.p.SaveProject.Request(uid()),
rpc.p.SaveProject.Response).result
event(ars, events.p.ProjectSaved)
assert ars.call_rpc(rpc.p.CloseProject.Request(uid()),
rpc.p.CloseProject.Response).result
event(ars, events.p.ProjectClosed)
event(ars, events.c.ShowMainScreen)
return project_evt.data.project
def test_project_const(start_processes: None, ars: ARServer) -> None:
event(ars, events.c.ShowMainScreen)
assert ars.call_rpc(
rpc.s.NewScene.Request(uid(),
rpc.s.NewScene.Request.Args("Test scene")),
rpc.s.NewScene.Response).result
scene_data = event(ars, events.s.OpenScene).data
assert scene_data
scene = scene_data.scene
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.s.AddObjectToScene.Request(
uid(),
rpc.s.AddObjectToScene.Request.Args("random_actions",
RandomActions.__name__)),
rpc.s.AddObjectToScene.Response,
).result
obj = event(ars, events.s.SceneObjectChanged).data
assert obj
# ------------------------------------------------------------------------------------------------------------------
assert ars.call_rpc(
rpc.p.NewProject.Request(
uid(), rpc.p.NewProject.Request.Args(scene.id, "Project name")),
rpc.p.NewProject.Response,
).result
proj = event(ars, events.p.OpenProject).data
assert proj
event(ars, events.s.SceneState)
assert ars.call_rpc(
rpc.p.AddConstant.Request(
uid(),
rpc.p.AddConstant.Request.Args("min_time", "double",
json.dumps(0.45))),
rpc.p.AddConstant.Response,
).result
c1 = event(ars, events.p.ProjectConstantChanged).data
assert c1
assert not ars.call_rpc(
rpc.p.AddConstant.Request(
uid(),
rpc.p.AddConstant.Request.Args("min_time", "double",
json.dumps(0.62))),
rpc.p.AddConstant.Response,
).result
assert not ars.call_rpc( # attempt to update without lock
rpc.p.UpdateConstant.Request(
uid(),
rpc.p.UpdateConstant.Request.Args(c1.id, name="min_time_updated")),
rpc.p.UpdateConstant.Response,
).result
# ------------------------------------------------------------------------------------------------------------------
# the user opens a menu and then closes it without actually changing anything
lock_object(ars, c1.id)
assert ars.call_rpc(
rpc.p.UpdateConstant.Request(uid(),
rpc.p.UpdateConstant.Request.Args(
c1.id, name="min_time_1"),
dry_run=True),
rpc.p.UpdateConstant.Response,
).result
assert ars.call_rpc(
rpc.p.UpdateConstant.Request(uid(),
rpc.p.UpdateConstant.Request.Args(
c1.id, name="min_time_2"),
dry_run=True),
rpc.p.UpdateConstant.Response,
).result
unlock_object(ars, c1.id)
# ------------------------------------------------------------------------------------------------------------------
lock_object(ars, c1.id)
assert ars.call_rpc(
rpc.p.UpdateConstant.Request(
uid(),
rpc.p.UpdateConstant.Request.Args(c1.id, name="min_time_updated")),
rpc.p.UpdateConstant.Response,
).result
c1u = event(ars, events.p.ProjectConstantChanged).data
assert c1u
event(ars, events.lk.ObjectsUnlocked)
assert c1u.id == c1.id
assert c1.name != c1u.name
assert c1.type == c1u.type
# ------------------------------------------------------------------------------------------------------------------
# try to add and remove
assert ars.call_rpc(
rpc.p.AddConstant.Request(
uid(),
rpc.p.AddConstant.Request.Args("min_time_2", "double",
json.dumps(0.62))),
rpc.p.AddConstant.Response,
).result
c2 = event(ars, events.p.ProjectConstantChanged).data
assert c2
assert ars.call_rpc(
rpc.p.RemoveConstant.Request(uid(),
rpc.p.RemoveConstant.Request.Args(c2.id)),
rpc.p.RemoveConstant.Response,
).result
c2e = event(ars, events.p.ProjectConstantChanged)
assert c2e.data
assert c2e.data.id == c2.id
assert c2e.change_type == c2e.Type.REMOVE
# ------------------------------------------------------------------------------------------------------------------
# attempt to add a constant with duplicate name
assert not ars.call_rpc(
rpc.p.AddConstant.Request(
uid(),
rpc.p.AddConstant.Request.Args(c1u.name, "double",
json.dumps(0.62))),
rpc.p.AddConstant.Response,
).result
# ------------------------------------------------------------------------------------------------------------------
assert ars.call_rpc(
rpc.p.AddActionPoint.Request(
uid(), rpc.p.AddActionPoint.Request.Args("ap1",
common.Position())),
rpc.p.AddActionPoint.Response,
).result
ap = event(ars, events.p.ActionPointChanged).data
assert ap is not None
assert ars.call_rpc(
rpc.p.AddAction.Request(
uid(),
rpc.p.AddAction.Request.Args(
ap.id,
"test_action",
f"{obj.id}/{RandomActions.random_double.__name__}",
[
common.ActionParameter(
"range_min", common.ActionParameter.TypeEnum.CONSTANT,
json.dumps(c1.id)),
common.ActionParameter("range_max", "double", "0.55"),
],
[common.Flow(outputs=["random_value"])],
),
),
rpc.p.AddAction.Response,
).result
action = event(ars, events.p.ActionChanged).data
assert action
assert not ars.call_rpc(
rpc.p.RemoveConstant.Request(uid(),
rpc.p.RemoveConstant.Request.Args(c1.id)),
rpc.p.RemoveConstant.Response).result
# ------------------------------------------------------------------------------------------------------------------
# try to execute action using constant parameter
assert ars.call_rpc((rpc.s.StartScene.Request(uid())),
rpc.s.StartScene.Response).result
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Starting
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Started
assert ars.call_rpc(
rpc.p.ExecuteAction.Request(
uid(), rpc.p.ExecuteAction.Request.Args(action.id)),
rpc.p.ExecuteAction.Response)
event(ars, events.a.ActionExecution)
res = event(ars, events.a.ActionResult)
assert res.data
assert res.data.action_id == action.id
assert not res.data.error
assert ars.call_rpc((rpc.s.StopScene.Request(uid())),
rpc.s.StopScene.Response).result
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Stopping
assert event(ars, events.s.SceneState
).data.state == events.s.SceneState.Data.StateEnum.Stopped
assert ars.call_rpc(
rpc.p.RemoveAction.Request(uid(), rpc.p.IdArgs(action.id)),
rpc.p.RemoveAction.Response).result
assert event(ars, events.p.ActionChanged).data
assert ars.call_rpc(
rpc.p.RemoveConstant.Request(uid(),
rpc.p.RemoveConstant.Request.Args(c1.id)),
rpc.p.RemoveConstant.Response).result
event(ars, events.p.ProjectConstantChanged)
