async def robot_joints_event(robot_inst: Robot) -> None:
logger.info(f"Sending joints for {robot_inst.name} started.")
while scene_started() and glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]:
start = time.monotonic()
try:
evt = RobotJoints(RobotJoints.Data(robot_inst.id, (await robot.get_robot_joints(robot_inst, None, True))))
except Arcor2Exception as e:
logger.error(f"Failed to get joints for {robot_inst.name}. {str(e)}")
await asyncio.sleep(1)
continue
evt_json = evt.to_json()
await asyncio.gather(
*[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]]
)
end = time.monotonic()
await asyncio.sleep(EVENT_PERIOD - (end - start))
ROBOT_JOINTS_TASKS.pop(robot_inst.id, None)
# TODO notify UIs that registration was cancelled
glob.ROBOT_JOINTS_REGISTERED_UIS.pop(robot_inst.id, None)
logger.info(f"Sending joints for {robot_inst.name} stopped.")
async def robot_eef_pose_event(robot_inst: Robot) -> None:
global EEF_POSE_TASKS
glob.logger.info(f"Sending '{sevts.r.RobotEef.__name__}' for robot '{robot_inst.id}' started.")
while scene_started() and glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]:
start = time.monotonic()
evt = sevts.r.RobotEef(sevts.r.RobotEef.Data(robot_inst.id))
try:
evt.data.end_effectors = await asyncio.gather(
*[eef_pose(robot_inst, eef_id) for eef_id in (await robot.get_end_effectors(robot_inst))]
)
except Arcor2Exception as e:
glob.logger.error(f"Failed to get eef pose for {robot_inst.id}. {str(e)}")
break
evt_json = evt.to_json()
await asyncio.gather(
*[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]]
)
end = time.monotonic()
await asyncio.sleep(EVENT_PERIOD - (end - start))
del EEF_POSE_TASKS[robot_inst.id]
# TODO notify UIs that registration was cancelled
del glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]
glob.logger.info(f"Sending '{sevts.r.RobotEef.__name__}' for robot '{robot_inst.id}' stopped.")
async def robot_joints_event(robot_inst: Robot) -> None:
global ROBOT_JOINTS_TASKS
glob.logger.info(f"Sending '{sevts.r.RobotJoints.__name__}' for robot '{robot_inst.id}' started.")
while scene_started() and glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]:
start = time.monotonic()
try:
evt = sevts.r.RobotJoints(
sevts.r.RobotJoints.Data(robot_inst.id, (await robot.get_robot_joints(robot_inst)))
)
except Arcor2Exception as e:
glob.logger.error(f"Failed to get joints for {robot_inst.id}. {str(e)}")
break
evt_json = evt.to_json()
await asyncio.gather(
*[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]]
)
end = time.monotonic()
await asyncio.sleep(EVENT_PERIOD - (end - start))
del ROBOT_JOINTS_TASKS[robot_inst.id]
# TODO notify UIs that registration was cancelled
del glob.ROBOT_JOINTS_REGISTERED_UIS[robot_inst.id]
glob.logger.info(f"Sending '{sevts.r.RobotJoints.__name__}' for robot '{robot_inst.id}' stopped.")
async def run_temp_package(package_id: str) -> None:
# TODO lock scene and project?
assert glob.LOCK.scene
assert glob.LOCK.project
project_id = glob.LOCK.project.id
glob.TEMPORARY_PACKAGE = True
scene_online = scene_started()
if scene_online:
await stop_scene(glob.LOCK.scene
) # the package will start it on its own
await project.close_project()
req = erpc.RunPackage.Request
exe_req = req(uuid.uuid4().int,
args=req.Args(package_id, cleanup_after_run=False))
exe_resp = await manager_request(exe_req)
if not exe_resp.result:
glob.logger.warning(
f"Execution of temporary package failed with: {exe_resp.messages}."
)
else:
await server_events.package_started.wait()
await server_events.package_stopped.wait()
glob.logger.info(
"Temporary package stopped, let's remove it and reopen project.")
glob.TEMPORARY_PACKAGE = False
await manager_request(
erpc.DeletePackage.Request(uuid.uuid4().int,
args=rpc.common.IdArgs(package_id)))
await project.open_project(project_id)
assert glob.LOCK.scene
assert glob.LOCK.project
await notif.broadcast_event(
sevts.p.OpenProject(
sevts.p.OpenProject.Data(glob.LOCK.scene.scene,
glob.LOCK.project.project)))
if scene_online:
await start_scene(glob.LOCK.scene)
async def robot_eef_pose_event(robot_inst: Robot) -> None:
eefs: dict[Optional[str], set[str]] = {}
try:
try:
for arm_id in await robot.get_arms(robot_inst):
eefs[arm_id] = await robot.get_end_effectors(robot_inst, arm_id)
except robot.SingleArmRobotException:
eefs = {None: await robot.get_end_effectors(robot_inst, None)}
except Arcor2Exception as e:
logger.error(f"Failed to start sending poses for {robot_inst.name}. {str(e)}")
else:
logger.info(f"Sending poses for {robot_inst.name} started. Arms/EEFs: {eefs}.")
while scene_started() and glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]:
start = time.monotonic()
evt = RobotEef(RobotEef.Data(robot_inst.id))
try:
evt.data.end_effectors = await asyncio.gather(
*[eef_pose(robot_inst, eef_id, arm_id) for arm_id in eefs.keys() for eef_id in eefs[arm_id]]
)
except Arcor2Exception as e:
logger.error(f"Failed to get eef pose for {robot_inst.name}. {str(e)}")
await asyncio.sleep(1)
continue
evt_json = evt.to_json()
await asyncio.gather(
*[ws_server.send_json_to_client(ui, evt_json) for ui in glob.ROBOT_EEF_REGISTERED_UIS[robot_inst.id]]
)
end = time.monotonic()
await asyncio.sleep(EVENT_PERIOD - (end - start))
EEF_POSE_TASKS.pop(robot_inst.id, None)
# TODO notify UIs that registration was cancelled
glob.ROBOT_EEF_REGISTERED_UIS.pop(robot_inst.id, None)
logger.info(f"Sending poses for {robot_inst.name} stopped.")
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
