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 broadcast_event(event: events.Event) -> None:
logger.debug(event)
if glob.USERS.interfaces:
message = event.to_json()
await asyncio.gather(*[ws_server.send_json_to_client(intf, message) for intf in glob.USERS.interfaces])
async def broadcast_event(
event: events.Event,
exclude_ui: Optional[WebSocketServerProtocol] = None) -> None:
if (exclude_ui is None
and glob.INTERFACES) or (exclude_ui and len(glob.INTERFACES) > 1):
message = event.to_json()
await asyncio.gather(*[
ws_server.send_json_to_client(intf, message)
for intf in glob.INTERFACES if intf != exclude_ui
])
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 handle_manager_incoming_messages(manager_client) -> None:
event_mapping: Dict[str, Type[events.Event]] = {
evt.__name__: evt
for evt in EXE_EVENTS
}
rpc_mapping: Dict[str, Type[rpc.common.RPC]] = {
r.__name__: r
for r in EXE_RPCS
}
try:
async for message in manager_client:
msg = json.loads(message)
if "event" in msg:
if glob.USERS.interfaces:
await asyncio.gather(*[
ws_server.send_json_to_client(intf, message)
for intf in glob.USERS.interfaces
])
try:
evt = event_mapping[msg["event"]].from_dict(msg)
except ValidationError as e:
glob.logger.error("Invalid event: {}, error: {}".format(
msg, e))
continue
if isinstance(evt, events.PackageInfo):
glob.PACKAGE_INFO = evt.data
elif isinstance(evt, events.PackageState):
glob.PACKAGE_STATE = evt.data
if evt.data.state == events.PackageState.Data.StateEnum.STOPPED:
if not glob.TEMPORARY_PACKAGE:
# after (ordinary) package is finished, show list of packages
glob.MAIN_SCREEN = evts.c.ShowMainScreen.Data(
evts.c.ShowMainScreen.Data.WhatEnum.
PackagesList)
await notif.broadcast_event(
evts.c.ShowMainScreen(
evts.c.ShowMainScreen.Data(
evts.c.ShowMainScreen.Data.WhatEnum.
PackagesList, evt.data.package_id)))
# temporary package is handled elsewhere
server_events.package_stopped.set()
server_events.package_started.clear()
glob.ACTION_STATE_BEFORE = None
glob.PACKAGE_INFO = None
else:
server_events.package_stopped.clear()
server_events.package_started.set()
elif isinstance(evt, events.ActionStateBefore):
glob.ACTION_STATE_BEFORE = evt.data
elif "response" in msg:
# TODO handle potential errors
rpc_cls = rpc_mapping[msg["response"]]
resp = rpc_cls.Response.from_dict(msg)
exe.MANAGER_RPC_RESPONSES[resp.id].put_nowait(resp)
except websockets.exceptions.ConnectionClosed:
glob.logger.error("Connection to manager closed.")
