
    ѩi                     Z   d dl mZ d dlZd dlZd dlZd dlZd dlZd dlZd dlZd dl	Z	d dl
mZ d dlmZ d dlmZmZmZmZmZmZ d dlZd dlmZ ddlmZ dd	lmZ dd
lmZmZmZm Z m!Z!m"Z"m#Z#m$Z$m%Z% ddl&m'Z' ddl(m)Z) ddl*m+Z+ ddl,m-Z-m.Z.  G d d      Z/ddZ0ddZ1ddZ2e3dk(  r e2        yy)    )annotationsN)
HTTPStatus)Path)AnyDictListOptionalSetTuple)WebSocketServerProtocol   )FrameCapture)FRLoop)	BatteryErrorMessageEventLogMessageGoalTypeMessageTypeNavStatusMessagePose
RobotStateWaypointListMessage)Pose2D)MockNavBackend)RosbridgeBackend)WaypointRegistryload_waypointsc                  ,   e Zd Z	 	 d	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 	 ddZddZddZddZd dZddddd	 	 	 	 	 	 	 	 	 	 	 d!d	Zd"d
Z	d#dZ
d$dZd"dZd%dZd$dZd&dZ	 	 	 	 	 	 d'dZd(dZd)d*dZd"dZd+dZd,dZd-dZd"dZddZd.dZd(dZd/dZy)0EdgeProxyServerNc	                   || _         || _        || _        || _        || _        t               | _        t        j                         | _	        t        |      | _        t        t        j                  dd            | _        |dk(  rt!               | _        n4|dk(  r!t%        |xs t'        dd      |      | _        nt)        d|       d | _        d | _        d	| _        d	| _        d
| _        d | _        t        j                  dd      }	t7        |	      | _        t        j                  dd      }
t        t        j                  dd            }t;        | j8                  |
|      | _        | j<                  j?                  | j@                         d| _!        t        t        j                  dd            | _"        tG        tI        t        j                  dd            d      | _%        t               | _&        t        j                         | _'        tQ        t        j                  dd            | _)        tQ        t        j                  dd            | _*        tG        tI        t        j                  dd            d      | _+        d| _,        g | _-        | j]                          t_        j`                  d       | _1        y )!N"EDGE_PROXY_RELATIVE_POSE_STALE_SECz3.0mock	rosbridgeEDGE_PROXY_ROSBRIDGE_URLwss://127.0.0.1:9090)rosbridge_urlinsecure_tlszUnknown backend:          FRAME_CAPTURE_URLz$http://192.168.168.105:8889/cam/jpeg)snapshot_urlFR_ENDPOINTzws://kluster.klass.dev:42067/FR_FPS5)frame_capturefr_endpointfpsr   FR_BROADCAST_SEND_TIMEOUT_SECz0.3FR_BROADCAST_LOG_EVERY10r   EDGE_PROXY_EVENT_JOURNAL_PATHz/tmp/edge_proxy_events.jsonlEDGE_PROXY_ARTIFACT_DIRz/tmp/edge_proxy_artifacts$EDGE_PROXY_EVENT_JOURNAL_MAX_ENTRIES500
   
edge_proxy)2hostportws_pathhealth_pathwaypoints_pathset_clientsasyncioLock_clients_lockr   
_waypointsfloatosgetenv_relative_pose_stale_secr   _backendr   _env
ValueError	_nav_task_active_request_id_active_destination
_nav_state_nav_progress_state_broadcast_taskr   _frame_capturer   _fr_loopon_detections_broadcast_fr_detections_fr_broadcast_count_fr_broadcast_send_timeoutmaxint_fr_broadcast_log_every_fr_clients_fr_clients_lockr   _event_journal_path_artifact_dir_event_journal_max_entries
_event_seq_event_journal_load_event_journallogging	getLoggerlogger)selfr;   r<   r=   r>   backendr?   r&   rosbridge_insecure_tls_snapshot_url_fr_endpoint_fr_fpss               ?/home/nelsen/Projects/kognitive/edge-proxy/edge_proxy/server.py__init__zEdgeProxyServer.__init__&   s[    		&,69e$\\^,:>,J(-bii8\^c.d(e%f*,DM#,+gt4NPf/g3DM
 0	:;;1515(* !$'=A" 		!G
 +F yy0OP		(C01--$

 	##D$A$AB#$ */II5u=+
' (+		2D9:A(
$ := ' $(II57UV$
  "II/1LM
 +.		@%HI+
' 46  "''5    c                z  K   | j                   j                          d {    t        j                  | j	                               | _        | j                  j                          d {    | j                  j                  d| j                  | j                         	 t        j                  | j                  | j                  | j                  dd| j                        4 d {    t        j                          d {    d d d       d {    | j                  j!                          | j"                  j%                          | j
                  j'                          	 | j
                   d {    y 7 ]7 7 7 7 r# 1 d {  7  sw Y   xY w7 &# t        j(                  $ r Y y w xY w# | j                  j!                          | j"                  j%                          | j
                  j'                          	 | j
                   d {  7   w # t        j(                  $ r Y w w xY wxY ww)Nz&Starting Edge Proxy WS server on %s:%s   )ping_intervalping_timeoutprocess_request)rJ   startrB   create_task_periodic_state_broadcastrR   rT   rf   infor;   r<   
websocketsserve_route_handler_process_requestFuturestoprS   closecancelCancelledErrorrg   s    rm   rz   zEdgeProxyServer.servey   s    mm!!###%,%8%89W9W9Y%Z"mm!!###A499diiX	!''##				  $ 5 5 ' ' nn&&&' ' MM %%'&&--/0000+ 	$ 	$' '' ' ' ' 1))  MM %%'&&--/0000)) s   H;E=A	H;*F +5H;!AF9 (F)F9 ,F	FF		F9 FF9 AH;(F  7F8F  <H; H;F9 F	F9 	FFFF9 F   F63H;5F66H;9AH8	HHHH8H52H84H55H88H;c                |   g | _         | j                  j                         sy	 | j                  j                  dd      5 }|D ]z  }|j	                         }|s	 t        j                  |      }t        |t              s=|j                  d      t        j                  k7  r`| j                   j                  |       | 	 ddd       t        | j                         | j                  kD  r | j                   | j                   d | _         yy# t        $ r Y w xY w# 1 sw Y   [xY w# t        $ r
 g | _         Y yw xY w)z0Load pending event logs from disk (best-effort).Nrutf-8encodingtype)rb   r^   existsopenstripjsonloads	Exception
isinstancedictgetr   	EVENT_LOGappendlenr`   )rg   handlerawentrys       rm   rc   z#EdgeProxyServer._load_event_journal   s2    ''..0	))..sW.E 6! 6C))+C ! $

3 &eT2 yy(K,A,AA ''..u566$ t""#d&E&EE"&"5"5t7V7V6V6X"YD F % ! !6 6  	"$D	sN   D( DD0ADD( 	DDDDD%!D( (D;:D;c                   	 | j                   j                  j                  dd       | j                   j                  dd      5 }| j                  D ]9  }|j                  t        j                  |d             |j                  d       ; 	 ddd       y# 1 sw Y   yxY w# t        $ r&}| j                  j                  d	|       Y d}~yd}~ww xY w)
z1Persist pending event logs to disk (best-effort).Tparentsexist_okwr   r   )ensure_ascii
Nz#Failed persisting event journal: %s)r^   parentmkdirr   rb   writer   dumpsr   rf   warning)rg   r   eventexcs       rm   _persist_event_journalz&EdgeProxyServer._persist_event_journal   s    	L$$++11$1N))..sW.E '!00 'ELLE!EFLL&'' ' '  	LKK EsKK	Ls7   AB% A	BB% B"B% "B% %	C.CCc                    | xj                   dz  c_         dt        t        j                         dz         d| j                    S )Nr   zedge-  -)ra   rZ   timer   s    rm   _next_event_idzEdgeProxyServer._next_event_id   s9    1s499;-./q0ABBro   r(   )
request_idstatuspayload	timestampc          
       K   t        | j                         |||t        ||nt        j                               d|xs i       j	                         }| j
                  j                  |       t        | j
                        | j                  kD  r| j
                  | j                   d  | _        | j                          | j                  |       d {    y 7 w)NF)event_id
event_typer   r   r   replayr   )r   r   rF   r   to_dictrb   r   r   r`   r   
_broadcast)rg   r   r   r   r   r   r   s          rm   _record_event_logz!EdgeProxyServer._record_event_log   s       ((*!!)>IDIIKPMr
 ') 	 	""5)t""#d&E&EE"&"5"5t7V7V6V6X"YD##% ooe$$$s   CCCCc                  K   | j                   syg }| j                   D ]6  }t        |      }d|d<   |j                  t        j                  |             8 	 |D ]  }|j                  |       d{     	 | j                  j                  dt        | j                                | j                   j                          | j                          y7 a# t        $ r&}| j                  j                  d|       Y d}~yd}~ww xY ww)z{Replay pending event logs to a newly connected client.

        We clear the journal only after all sends succeed.
        NTr   zEvent replay interrupted: %szReplayed %d pending edge events)rb   r   r   r   r   sendr   rf   r   rx   r   clearr   )rg   	websocketpayloadsr   replay_eventr   r   s          rm   _replay_event_journalz%EdgeProxyServer._replay_event_journal   s     
 "" (( 	6E;L%)L"OODJJ|45	6
	# .nnW---. 	:C@S@S<TU!!###% . 	KK >D	sC   ADC 1C2C 8ADC 	DC?:D?DDc          
     r  K   t        j                         }	 |j                  d| j                  j                         d{   }|s!| j                  d|d|dd       d{    yt        t        j                         dz        }d	j                  d
 |D              }| d|xs d d|xs d d}| j                  |z  }		 | j                  j                  dd       |	j                  |       | j                  d|d|t        |	      t!        |      d       d{    y7 # t
        $ r)}| j                  j                  d|       d}Y d}~d}~ww xY w7 7 ># t
        $ rJ}| j                  j                  d|       | j                  d|d|d| d       d{  7   Y d}~yd}~ww xY ww)z5Capture and save a snapshot when reaching a waypoint.Nz#arrival artifact capture failed: %sscan_area_capturefailedcapture_failed)destinationreasonr   r   r   r   r   r(   c              3  L   K   | ]  }|j                         s|d v r|nd  yw)>   _r   r   N)isalnum).0chs     rm   	<genexpr>z<EdgeProxyServer._capture_arrival_artifact.<locals>.<genexpr>	  s%     bPR"**,"
2BBKbs   "$r   navunknownz.jpgTr   saved)r   path
size_bytesz arrival artifact save failed: %szsave_failed:)rB   get_event_looprun_in_executorrS   capturer   rf   r   r   rZ   r   joinr_   r   write_bytesstrr   )
rg   r   r   loopframer   now_ms	safe_destfilenametargets
             rm   _capture_arrival_artifactz)EdgeProxyServer._capture_arrival_artifact   s    %%'	..tT5H5H5P5PQQE
 ((.%#..	 )    TYY[4'(GGbVabb	XQz2U31Y5K)4LDQ##h.	$$TD$Au%((.%#.K"%e*	 ) 	 	 	1 R 	KK EsKE	
$	  
	KK BCH((.%#. ,SE2	 )   
	s   F7)D( D&D( F7#E$AF7AE!  E!E! %F7&D( (	E1EF7EF7E! !	F4*:F/$F'%F/*F7/F44F7c                   K   |dk(  r| j                  |       d{    y| j                  ||       d{    y7  7 w)z0Route incoming WS connections based on URL path./frN)_fr_handler_handler)rg   r   r   s      rm   r{   zEdgeProxyServer._route_handler%  s>     5=""9-----	4000 .0s   A=A?AAc           	       K   | j                   j                  d|j                         | j                  4 d{    | j                  j                  |       ddd      d{    	 |2 3 d{   }
7 <7 # 1 d{  7  sw Y   #xY w7 6 n2# t        $ r&}| j                   j                  d|       Y d}~nd}~ww xY w| j                  4 d{  7   | j                  j                  |       ddd      d{  7   n# 1 d{  7  sw Y   nxY w| j                   j                  d|j                         y# | j                  4 d{  7   | j                  j                  |       ddd      d{  7   n# 1 d{  7  sw Y   nxY w| j                   j                  d|j                         w xY ww)zRead-only endpoint for dashboard FR overlay.

        Clients connect and receive ``fr_detections`` messages.  Any incoming
        messages are silently ignored.
        zFR client connected: %sNzFR client error: %szFR client disconnected: %s)	rf   rx   remote_addressr]   r\   addr   debugdiscard)rg   r   r   r   s       rm   r   zEdgeProxyServer._fr_handler,  su     	2I4L4LM(( 	, 	,  +	, 	,	U$  a		, 	, 	, 	, 	,9 	:KK3S99	: ,, 4 4  ((34 4 4 4 4KK99;S;ST ,, 4 4  ((34 4 4 4 4KK99;S;STs"  7GA6GA:G$A8%G*B ,B0B1B4B 6G8G:B BBGBB E 	CB=8E =CE GCGD6GDGDDD.GGE
GF6GFGFFF.GGc                   K   i }t         j                  dd      }t        |      r	  |       }t        |t              r|}t        j                  |t        j                         d}|r||d<   t        j                  |       j                  4 d{    t         j                        }ddd      d{     j                  4 d{    t         j                        }ddd      d{    z   }|syt        j                          }	g }
g }d fdt#        j$                  fd|D          d{   }t'        |      }d}|D ]4  \  }}|r|dz  }||v r|
j)                  |       $|j)                  |       6 |
rG j                  4 d{    |
D ]  } j                  j+                  |        ddd      d{    |rG j                  4 d{    |D ]  } j                  j+                  |        ddd      d{     xj,                  dz  c_         j,                   j.                  z  dk(  rt        j                          |	z
  d	z  }dd
} j0                  j3                  d j,                  t5        |      t5        |      |t5        |
      t5        |      z    ||j7                  d             ||j7                  d             ||j7                  d             ||j7                  d             ||j7                  d             ||      t5        |             yy# t
        $ r Y w xY w7 7 # 1 d{  7  sw Y   xY w7 7 w# 1 d{  7  sw Y   xY w7 F7 7 # 1 d{  7  sw Y   xY w7 7 # 1 d{  7  sw Y   xY ww)zABroadcast FR detections to all /fr clients AND all /edge clients.get_last_cycle_metricsN)r   
detectionsr   metricsc                   K   	 t        j                  | j                        j                         d {    | dfS 7 # t        $ r | dfcY S w xY ww)N)timeoutTF)rB   wait_forr   rX   r   )wsr   rg   s    rm   	_send_onez;EdgeProxyServer._broadcast_fr_detections.<locals>._send_oneb  sc     !&&GGG$ ;;   4x	
  !5y !s8   A3A AA  AA AAAAc              3  .   K   | ]  } |        y wN )r   r   r   s     rm   r   z;EdgeProxyServer._broadcast_fr_detections.<locals>.<genexpr>l  s     (M22(Ms   r   r   g     @@c                N    t        | t        t        f      rt        |       ddS y)N.1fmszn/a)r   rZ   rF   )values    rm   _fmt_msz9EdgeProxyServer._broadcast_fr_detections.<locals>._fmt_ms  s'    ec5\2#El3/r22ro   zFR pipeline #%d: clients(fr=%d edge=%d) sent=%d dropped=%d capture=%s frame_age=%s fr_rtt=%s fr_server=%s edge_after_fr=%s broadcast=%s detections=%d
capture_msframe_age_msfr_roundtrip_msfr_server_total_msedge_since_fr_receive_ms)r   r   returnz$tuple[WebSocketServerProtocol, bool])r   r   r   r   )getattrrT   callabler   r   r   r   FR_DETECTIONSr   r   r   r]   listr\   rD   rA   	monotonicrB   gatherr@   r   r   rW   r[   rf   rx   r   r   )rg   r   
fr_metricsget_metricsr   payload_obj
fr_clientsedge_clientsall_clientst0dead_fr	dead_edgeresultsfr_set
sent_countr   okbroadcast_msr   r   r   s   `                  @@rm   rV   z(EdgeProxyServer._broadcast_fr_detections?  sz    %'
dmm-EtLK %-gt,!(J
  --$

 %/K	"**[) (( 	0 	0d../J	0 	0%% 	/ 	/.L	/ 	/ !</^^		!  (M(MNNZ
 	%FBa
V|r"  $	% ,, 1 1! 1B$$,,R011 1 )) . .# .BMM))"-.. . 	  A% ##d&B&BBaG NN,r1V;L
 KK h((JL!Gs9~-
|45
~67
'89:
';<=
'ABC%J Hu  	0 	0 	0 	0 	0	/ 	/ 	/ 	/ 	/* O1 1 1 1 1. . . . .sF  %O L= AO MO M0O ;M<O M)O M/*O 5M,6A	O ?N AO NO  #NO NO %N$&O )#N*O N'D%O =	M
O 	M

O O M&MM&!	O ,O /N5M86N=	O O O N!NN!	O 'O *N=0N31N=8O c           	       K   |d| j                   fvr|j                  dd       d {    y | j                  j                  d|j                         | j
                  4 d {    | j                  j                  |       d d d       d {    	 | j                  |       d {    | j                  |       d {    | j                  |       d {    |2 3 d {   }| j                  ||       d {    $7 7 7 z# 1 d {  7  sw Y   xY w7 w7 `7 I7 @7 (6 n2# t        $ r&}| j                  j                  d|       Y d }~nd }~ww xY w| j
                  4 d {  7   | j                  j                  |       d d d       d {  7   n# 1 d {  7  sw Y   nxY w| j                  j                  d|j                         y # | j
                  4 d {  7   | j                  j                  |       d d d       d {  7   n# 1 d {  7  sw Y   nxY w| j                  j                  d|j                         w xY ww)N/i  zUnsupported path)coder   zClient connected: %szClient error: %szClient disconnected: %s)r=   r   rf   rx   r   rD   rA   r   _send_waypoint_list_send_robot_stater   _handle_messager   r   r   )rg   r   r   messager   s        rm   r   zEdgeProxyServer._handler  s    T\\**//t4F/GGG/1I1IJ%% 	) 	)MMi(	) 	)	R**9555((333,,Y777!* ? ?g**9g>>> H	) 	) 	) 	) 	)
 637?> "+ 	9KK 2C88	9 )) 1 1%%i01 1 1 1 1KK6	8P8PQ )) 1 1%%i01 1 1 1 1KK6	8P8PQs}  'I"D;I"%D	&I")DI"DI"D. *D"+D. D$D. D&D. #D,'D((D,+D. D*D. I"	I"I"DDDI""D. $D. &D. (D,*D. ,D. -G .	E7EG EG  I"0E31I"5F#I"FI"#F5)F,*F51.I"I0G3
1I5H#IHI#H5)H,*H51.II"c                F  K   t        |t        t        f      r	 |j                  dd      }t        |t              s| j                  |ddd	       d {    y 	 t        j                  |      }t        |t              s| j                  |ddd       d {    y |j                  d      }|s| j                  |ddd       d {    y |t        j                  k(  r0|j                  t        j                  ddd             d {    y |t        j                   k(  r| j#                  |       d {    y |t        j$                  k(  r| j'                  |       d {    y |t        j(                  k(  r@|j                  dd      }|j                  dd      }| j+                  ||       d {    y |t        j,                  k(  r| j/                  ||       d {    y | j                  ||j                  dd      dt        |             d {    y # t        $ r! | j                  |ddd       d {  7   Y y w xY w7 # t        j                  $ r! | j                  |dd
d       d {  7   Y y w xY w7 7 7 7 [7 17 7 7 w)Nr   replace)errorsr(   invalid_byteszinvalid bytes)r   errormsgunsupported_messageunsupportedinvalid_jsonzinvalid jsoninvalid_payloadzpayload must be objectr   missing_typezmissing typepongr  )r   stater   r   r   r   unknown_message)r   bytes	bytearraydecoder   _send_errorr   r   r   JSONDecodeErrorr   r   r   PINGr   r   	GET_STATEr  NAVIGATE_handle_navigateCANCEL_NAVIGATION_handle_cancelCAPTURE_FRAME_handle_capture_frame)rg   r   r  r   msg_typer   r   s          rm   r  zEdgeProxyServer._handle_message  s    gy12!...C
 '3'""9CX^k"lll	jj)G
 '4(""9CTZr"sss;;v&""9>We"fff{'''..Vd,K!LMMM{,,,((333{+++''000{444 \26J[[2.F%%F%KKK{000,,Y@@@yW[[r5RZkqtu}q~[  &&yR\k&lll
 m
 ## 	""9>We"fff	
 t
 g N 4 1 L A 	@s   J!H% (J!IJ!I 1(J!J0J!
JAJ!J,J!:J;,J!'J(AJ!:J;-J!(J)6J!J J!%!II	IJ!IJ!+J	 JJ	J!J		J!J!J!J!J!J!J!J!c                x  K   |j                  dd      }t        j                         }	 |j                  d| j                  j
                         d{   }|Zt        j                  |      j                  d      }d||d}| j                  j                  dt        |      t        |             n!d|d	d
}| j                  j                  d       |j                  t        j                   |             d{    y7 # t        $ r(}| j                  j                  d|       d}Y d}~d}~ww xY w7 ;w)zHandle a capture_frame request and send frame_response back.

        Runs the synchronous FrameCapture.capture() in an executor so the
        event loop is not blocked during the network/subprocess call.
        r   r(   Nz2capture_frame: unexpected error during capture: %sr   frame_response)r   r   jpeg_b64z0capture_frame: sending %d-byte JPEG (b64 len=%d)r   )r   r   r  u<   capture_frame: no frame available — sending capture_failed)r   rB   r   r   rS   r   r   rf   r   base64	b64encoder)  r   r   r   r   r   )	rg   r   r   r   r   r   r   b64responses	            rm   r3  z%EdgeProxyServer._handle_capture_frame  s#     [[r2
%%'	+/+?+?d))11, &E ""5)009C(((H
 KKBCJPSTWPX
 )()H
 KK ^_nnTZZ12223&  	KK TVYZE	, 	3sM   'D:)D DD B$D:<D8=D:D 	D5D0+D:0D55D:c                   K   t        |j                  dd            j                         xs# dt        t	        j                         dz         |j                  d      xs i }|j                  dd      |j                  d      }dd d	|t
        j                  k(  s|d
k(  rt        |j                  dd            j                          j                  j                        }|s j                  t        dd       j                                d {     j                  t        dddd      j                                d {     j                  dddddd       d {    y t        |j                  |j                  |j                         n|t
        j"                  k(  s|dk(  r]dt        t%        |j                  dd            t%        |j                  dd            t%        |j                  dd                  nK|t
        j&                  k(  s|dk(  rt        |j                  dd            j                         j)                         }	 t%        |j                  dd            }|dvs|dk  r j                  t        d d!|d"|j                  d            j                                d {     j                  t        dddd d#      j                                d {     j                  dddd d#dd       d {    y  j-                         }| j                  t        d$d% j.                  d&d'      j                                d {     j                  t        dddd$d(      j                                d {     j                  dddd$d(dd       d {    y t%        |j                         }d)| d*|d+|d,k(  r|d}
}	n|d-k(  r| d}
}	n|d.k(  rd|}
}	nd| }
}	t1        j2                  |      |	z  t1        j4                  |      |
z  z
  }t1        j4                  |      |	z  t1        j2                  |      |
z  z   }t        t%        |j                        |z   t%        |j                        |z   |      n j                  t        d/d0|       j                                d {     j                  t        dt        |xs d      dd/d#      j                                d {     j                  ddt        |xs d      d/d#dd       d {    y  j6                  rK j6                  j9                         s1 j:                  r% j=                   j:                  d12       d {     _         _        d3 _         d _!        d6 fd4d6 fd5}tE        jF                   |              _        y 7 7 7 # t*        $ r d}Y w xY w7 U7 "7 7 7 ~7 _7 L7 7 7 w)7Nr   r(   nav_r   goalspeednormalr   mapwaypointnamewaypoint_not_foundzUnknown waypoint: r   r  r  r   r)   NAV_WAYPOINT_NOT_FOUNDr   r   r   progressr   
error_code
nav_status)r   r   rI  rH  r   )xythetaposerK  rL  rM  relative	directiondistanceg      >   leftrightforwardbackwardinvalid_goalz!Invalid relative goal: direction=z, distance=NAV_INVALID_GOALlocalization_unavailablezWCannot resolve relative navigation: missing or stale robot pose (required freshness <= r   zs)NAV_LOCALIZATION_LOSTz	relative::grT  rU  rR  unsupported_goalzUnsupported goal type: 	preemptedr%  acceptedc           
       K   j                   k7  ry | j                  _        | j                  t	        | j                        _        t        | j                  | j                  t	        | j                        nj
                  | j                  | j                        }j                  |j                                d {    | j                  dv rJj                  d| j                  |j                  | j                  | j                  d       d {    | j                  dk(  rj                         d {    y y 7 7 27 
w)NrG  >   r   arrivedr^  	cancelledrJ  )r   rH  r   rI  r   r`  )r   r   )rN   r   rP   rH  rF   rQ   r   r   rI  r   r   r   r   )updater  r   r   rg   s     rm   	on_updatez3EdgeProxyServer._handle_navigate.<locals>.on_update  s:    &&*4$mmDO*%*6??%;""%}}'39??3Nv/TXTfTf}}!,,C //#++-000}} NN,,+)!=='2$'LL"(--&,&7&7		 - 
 
 
 }}	)44) + 5    * 1
s7   CEEAEE )E	E
EEEc                   K   	 j                   j                  t                     d {    y 7 # t        $ r9}   t	        dddd t        |       d d                    d {  7   Y d } ~ y d } ~ ww xY ww)N)r   r   rN  r?  rc  frame_idUr   r   )r   rH  r   rI  )rJ   navigate_to_poser   r   r   )r   r   re  rc  rN  r   rg   r?  s    rm   run_navz1EdgeProxyServer._handle_navigate.<locals>.run_nav  s     mm44) +e*'% 5     qDbX4SVWZS[ko"pqs  sC   A?-: 8: A?: 	A<)A7,A/-A72A?7A<<A?r   None)$r   r   r   rZ   r   r   WAYPOINTrE   r   r   r   r   r   r   rK  rL  rM  POSErF   RELATIVElowerr   _relative_anchor_poserI   mathcossinrM   donerN   r1  rO   rP   rQ   rB   rv   )rg   r   r>  	goal_typewprP  rQ  anchor_poserM  dx_bodydy_bodydx_mapdy_maprh  r   re  rc  rN  r   r?  s   `             @@@@@@rm   r/  z EdgeProxyServer._handle_navigate  s    \267==?aT#diikZ^N^J_I`Ca
{{6"(bGX.HHV$	!%)))Y*-Ddhhvr2399;K$$[1Boo #-2"4[M B gi   oo$#-'$/!$3#; gi	 	 	 ,,+)#'2"6&>$'		 - 
 
 
 BDDBDD9D(--'9+> Kc*+c*+DHHWc23D
 (+++yJ/FDHH["56<<>DDFI  *c!:;  HHHX[Ooo #-,"CI=P[\`\d\deo\p[s t gi   oo$#-'$.!$-#5 gi	 	 	 ,,+)#'1"0&8$'		 - 
 
 
 446K"oo #-866:6S6STW5XXZ\	 gi	 	 	 oo$#-'$.!$9#: gi	 	 	 ,,+)#'1"<&=$'		 - 
 
 
 +++,E%i[(1>KI%#+Sj($,9cf$#&#&	 XXe_w.%71JJFXXe_w.%71JJF&/&/D //),5i[A ')   // )# #IO 4 -1 ')	 	 	 (('%#&yB#70"4 #		 ) 
 
 
  >>$.."5"5"7D<S<S%%1H1HQ\%]]]",#. $ #	J	 	 !,,WY7}	
2    	
		
B	
 ^s  DY&X'5YX!Y>X?C8Y8X" AYX4 5YX7!Y7X:8AYX=5YY !Y$Y%DY:Y;AY=Y	>.Y,Y-AYYAYYY"X1-Y0X11Y7Y:Y=Y YYY	YYYc                v  K   t        |xs d      j                         }|sy 	 | j                  j                  ||       d {    | j                  |k(  rd| _        | j                  t        |d| j                  | j                  t        |xs d      xs d       j                                d {    | j                  d|d| j                  | j                  t        |xs d      xs d d       d {    y y 7 # t        $ r&}| j
                  j                  d|       Y d }~d }~ww xY w7 7 <w)	Nr(   r%  zCancel failed: %sra  )r   r   r   rH  r   rJ  )r   rH  r   r   )r   r   rJ   r   r   rf   r   rN   rP   r   r   rO   rQ   r   r   )rg   r   r   r   s       rm   r1  zEdgeProxyServer._handle_cancel  s;    )r*002
	:--&&*V&LLL ""j0)DO// )& $ 8 8!//v|,4 ')   (('%"#'#;#; $ 2 2!&,B/74	 ) 	 	 	 1	 M 	:KK 3S99	:
	s`   !D9 D DD 	A+D94D55AD9:D7;D9D 	D2D-(D9-D22D97D9c                   K   t        | j                  j                               }|j                  t	        j
                  |j                                      d {    y 7 w)N)	waypoints)r   rE   r   r   r   r   r   rg   r   r  s      rm   r  z#EdgeProxyServer._send_waypoint_list  s>     !DOO,@,@,BCnnTZZ6777s   AA$A"A$c                    d}d}|}| j                   j                         D ]Z  }t        j                  |j                  |j                  z
  |j
                  |j
                  z
        }||k  sM|}|j                  }\ |S )zDReturn the name of the nearest waypoint within 2 m, or empty string.       @r(   )rE   r   rp  hypotrK  rL  rC  )rg   rN  _LOCATION_RADIUS_M	best_name	best_distru  dists          rm   _infer_locationzEdgeProxyServer._infer_location  st     	&	//&&( 	$B::bddTVVmRTTDFF];Di 	GG			$ ro   c                    t        | j                  d      s
t               S | j                  j                         }t	        | j                  dd      }t        |      r |       }||| j                  kD  ry|S )zReturn a safe anchor pose for resolving relative goals.

        If backend exposes pose age, require it to be fresh to avoid relative
        movement from stale localization.
        get_poseget_pose_age_secN)hasattrrJ   r   r  r   r   rI   )rg   rN  r  ages       rm   ro  z%EdgeProxyServer._relative_anchor_pose+  sj     t}}j16M}}%%'"4==2DdK$%"$C{cD$A$AAro   c           	        t        | j                  d      r| j                  j                         }n
t               }t        | j                  d      r| j                  j	                         }n
t               }| j                  |      }t        t        j                         |||| j                  | j                  | j                        S )zBuild a RobotState from live backend telemetry.

        RosbridgeBackend exposes get_pose() / get_battery(); MockNavBackend may
        not, so we fall back to zero-value defaults in that case.
        r  get_battery)r   rN  locationbattery	nav_statenav_progressnav_destination)r  rJ   r  r   r  r   r  r   r   rP   rQ   rO   )rg   rN  r  r  s       rm   _build_robot_state_messagez*EdgeProxyServer._build_robot_state_message>  s     4==*-==))+D6D4==-0mm//1GiG''-iikoo++ 44
 	
ro   c                   K   | j                         }|j                  t        j                  |j	                                      d {    y 7 wr   )r  r   r   r   r   r~  s      rm   r  z!EdgeProxyServer._send_robot_stateZ  s5     --/nnTZZ6777s   AAA	Ac                   K   	 t        j                  d       d{    | j                  s+| j                         }| j	                  |j                                d{    b7 I7 w)z?Broadcast robot_state to all connected clients every 2 seconds.r  N)rB   sleeprA   r  r   r   )rg   r  s     rm   rw   z)EdgeProxyServer._periodic_state_broadcast^  sY     --$$$==113C//#++-000 $ 1s"   A*A&AA* A(!A*(A*c           	        K   |j                  t        j                  t        |||      j	                                      d {    y 7 w)NrE  )r   r   r   r   r   )rg   r   r   r  r  s        rm   r*  zEdgeProxyServer._send_errorg  s5     nnTZZ
RWad(e(m(m(opqqqs   AAA
Ac                ,  K   t        j                  |      }| j                  4 d {    t        | j                        }d d d       d {    g }D ]  }	 |j                  |       d {     |rH| j                  4 d {    |D ]  }| j                  j                  |        d d d       d {    y y 7 7 t# 1 d {  7  sw Y   xY w7 j# t        $ r |j                  |       Y w xY w7 s7 C# 1 d {  7  sw Y   y xY wwr   )	r   r   rD   r   rA   r   r   r   r   )rg   r   r  clientsdeadr   s         rm   r   zEdgeProxyServer._broadcastj  s    **W%%% 	* 	*4==)G	* 	*  	 B ggg&&&	  )) . . .BMM))"-.. . . 	* 	* 	* 	* 	* '  B . . . . .s   &DC DCDCDC.C/C3DC;D#C?.D9C=:DDC
CCDCC85D7C88D=D?DDDDc                    || j                   k(  r-d}ddt        t        |            fg}t        j                  ||fS |d| j
                  dfvr-d}ddt        t        |            fg}t        j                  ||fS y )Ns   OK)zContent-Typez
text/plainzContent-Lengthr  r   s	   Not Found)r>   r   r   r   OKr=   	NOT_FOUND)rg   r   _request_headersbodyheaderss        rm   r|   z EdgeProxyServer._process_request{  s    4###D58H#cRVi.7YZG=='4//T\\511D58H#cRVi.7YZG''$66ro   )NT)r;   r   r<   rZ   r=   r   r>   r   rh   r   r?   r   r&   zOptional[str]ri   boolr   rj  ri  )r   r   )r   r   r   r   r   r   r   zOptional[Dict[str, Any]]r   zOptional[float]r   rj  )r   r   r   rj  )r   r   r   r   r   rj  )r   r   r   r   r   rj  )r   r   r   rj  )r   r   r  r   r   rj  )r   r   r   Dict[str, Any]r   rj  )r   r  r   rj  )r(   )r   r   r   r   r   rj  )rN  r   r   r   )r   zOptional[Pose])r   r   )
r   r   r   r   r  r   r  r   r   rj  )r   r   r  r   r   z(Optional[Tuple[HTTPStatus, list, bytes]])__name__
__module____qualname__rn   rz   rc   r   r   r   r   r   r{   r   rV   r   r  r3  r/  r1  r  r  ro  r  r  rw   r*  r   r|   r   ro   rm   r   r   %   s[    (,'+Q6Q6 Q6 	Q6
 Q6 Q6 Q6 %Q6 !%Q6 
Q6f4Z:	LC ,0%)% % 	%
 % *% #% 
%6&20d1U&YvR01@f$30$3;I$3	$3Ll8\@8&
881r."ro   r   c                8    t        j                  |       }||S |S r   )rG   rH   rC  defaultvs      rm   rK   rK     s    
		$AyHro   c                t    t        j                  |       }||S |j                         j                         dv S )N>   1onyestrue)rG   rH   r   rn  r  s      rm   	_env_boolr    s2    
		$Ay779?? :::ro   c                     t        j                  d      } | j                  dt        dd             | j                  dt        t	        t        dd	            
       | j                  dt        dd             | j                  dt        dd             | j                  dddgt        dd             | j                  dt        dd             | j                  dt        dd             | j                  dt        t        dd            j                         dd g!       | j                         }t        j                  t        j                  d"#       t        |j                  |j                  |j                  |j                   |j"                  |j$                  |j&                  t        |j(                        j                         dk(  $      }	 t+        j,                  |j/                                y # t0        $ r Y y w xY w)%NzEdge Proxy WebSocket server)descriptionz--hostEDGE_PROXY_WS_HOSTz0.0.0.0)r  z--portEDGE_PROXY_WS_PORT8080)r   r  z	--ws-pathEDGE_PROXY_WS_PATHz/edgez--health-pathEDGE_PROXY_HEALTH_PATHz/healthz	--backendr"   r#   EDGE_PROXY_BACKEND)choicesr  z--waypointsEDGE_PROXY_WAYPOINTS_PATHz/data/waypoints.yamlz--rosbridge-urlr$   r%   z--rosbridge-insecure-tls!EDGE_PROXY_ROSBRIDGE_INSECURE_TLSTr  false)r  r  z/%(asctime)s %(levelname)s %(name)s: %(message)s)levelformat)r;   r<   r=   r>   rh   r?   r&   ri   )argparseArgumentParseradd_argumentrK   rZ   r   r  rn  
parse_argsrd   basicConfigINFOr   r;   r<   r=   r>   rh   r}  r&   ri   rB   runrz   KeyboardInterrupt)parserargsservers      rm   mainr    s   $$1NOF
$/CY*OP
sC=QSY8Z4[\
T2F-PQ
6NPY1Z[
%);7  
 t4OQg/hi
/1GH   "IA4HIOOQ!   Dgll3deYYYY$$~~(("4#>#>?EEG6Q	FFLLN# s   #H 	HH__main__)rC  r   r  r   r   r   )rC  r   r  r  r   r  ri  )4
__future__r   r  rB   r8  r   rd   rp  rG   r   httpr   pathlibr   typingr   r   r   r	   r
   r   ry   websockets.serverr   r/   r   fr_loopr   messagesr   r   r   r   r   r   r   r   r   nav_backend.baser   nav_backend.mock_backendr   nav_backend.rosbridge_backendr   r}  r   r   r   rK   r  r  r  r   ro   rm   <module>r     s    "       	    8 8  5 ' 
 
 
 % 4 ; 7a aH;(V zF ro   