iddlmZddlZddlZddlZddlZddlmZmZddl m Z ddl Z ddl Z ddl mZmZmZmZmZmZmZmZddlmZmZmZddlmZmZddlmZmZdd l m!Z!m"Z"dd l#m$Z$m%Z%Gd d e&e Z'eGd dZ(Gdde)Z*eee&efeeee&dfgeee&efeee&efffZ+eee&efgdfZ,eGddZ-eGddZ.GddZ/dddZ0y)) annotationsN) dataclassfield)Enum)Any AwaitableCallableDictIterableListOptionalUnion) DAGParser PlanGraph ParseResult)PeriodicTriggerTriggerCondition) TTSClient TTSConfig)FRClientFRConfig) VLMClient VLMConfigc$eZdZdZdZdZdZdZdZy)NavStatezhNavigation state for Edge Proxy integration. Tracks the state of an active navigation command. accepted navigatingarrivedfailed cancelledN) __name__ __module__ __qualname____doc__ACCEPTED NAVIGATINGARRIVEDFAILED CANCELLEDL/home/nelsen/Projects/kognitive/orchestrator/src/executor/action_executor.pyrrs" HJG FIr+rcVeZdZUdZded<ded<ded<ded<dZd ed <d Zd ed <y )NavigationContextzuContext for an active navigation operation. Stores state for the current navigation command being executed. str request_id destinationspeedrstatusfloatprogressN Optional[str]error)r!r"r#r$__annotations__r6r8r*r+r,r.r.&s3 O J HeE=r+r.c eZdZy)ActionExecutionErrorN)r!r"r#r*r+r,r;r;5sr+r; StepOutcomecVeZdZUded<ded<ded<eeZded<dZd ed <y) r<r/step_idactionr3default_factoryDict[str, Any]outputNr7r8)r!r"r#r9rdictrCr8r*r+r,r<r<@s* L K K"48FN8E=r+cHeZdZUded<ded<ded<eeZded <y ) ExecutionReportr/plan_idboolsuccessDict[str, StepOutcome]resultsr@z List[str]errorsN)r!r"r#r9rlistrLr*r+r,rFrFIs! L M ##d3FI3r+rFceZdZdZ d# d$dZ d%dZd&dZd'd(dZd)dZd*dZ d+d Z d)d Z d)d Z d)d Z d,d Zd)dZd-dZd.dZd/dZd-dZ d0 d1dZd2dZ d3dZd4dZd5dZ d6dZ d7dZ d8dZ d9 d:dZ d7dZ d7dZ d7dZ d7dZ d7d Z!d;d&>?vz* 7 78v12''(B(BC3 r+c:|jj|y)ziRegister a periodic trigger. Args: trigger: PeriodicTrigger instance to add N)rqappendrxtriggers r, add_triggerzActionExecutor.add_triggers g&r+cz|jD]}|j|jjy)zClear all registered triggers.N)rqstopclearrs r,clear_triggerszActionExecutor.clear_triggerss.~~ G LLN  r+cNK|jD]}|jyw)zStart all periodic triggers.N)rqstartrs r,_start_triggerszActionExecutor._start_triggerss"~~ G MMO #%cNK|jD]}|jyw)zStop all periodic triggers.N)rqrrs r,_stop_triggerszActionExecutor._stop_triggerss"~~ G LLN rcPK|jD]}|j|yw)znUpdate progress for all triggers. Args: progress: Progress value from 0.0 to 1.0 N)rqupdate_progress)rxr6rs r,_update_trigger_progressz'ActionExecutor._update_trigger_progresss) ~~ .G  # #H - .s$&cNK|jD]}|jyw)z0Notify all triggers that navigation is complete.N)rqon_navigation_completers r,$_notify_triggers_navigation_completez3ActionExecutor._notify_triggers_navigation_completes%~~ -G  * * , -rc4K|jyt|dd}|r||jjk7ryt|dr |jnd}|dk(r t j |j_y|dk(rkt j|j_t|dr?|j|j_|j|jd{yy|dk(rJt j|j_d |j_|jd d{y|d k(rUt j|j_t|d r)|jr|j|j_ yyy|d k(r|t j|j_t|dr|j|j_t|d r)|jr|j|j_ yyyy727w) z|Handle navigation status updates from Edge Proxy. Args: msg: NavStatusMessage from Edge Proxy. Nr0r_r3rrr6r?rrr )rngetattrr0rr3rr%r&r6rr'r(rr8r))rxmsgmsg_request_id status_strs r,rz!ActionExecutor._handle_nav_statuss    $  lB7 n0A0A0L0LL $+3#9SZZr  #'/'8'8D   $ < ''/':':D   $sJ'-0\\!!*33CLLAAA(9 $'/'7'7D   $),D   &//4 4 4 8 #'/D   $sH%#***-**!!'+5% ; &'/'9'9D   $sJ'-0\\!!*sH%#***-**!!'+5% 'B 5s&CH!H"AH1H2C"HHcHKt|dr|j|_yyw)zrHandle robot state updates from Edge Proxy. Args: state: RobotState from Edge Proxy. locationN)rrro)rxstates r,rz"ActionExecutor._update_robot_states# 5* %#(>>D  &s "cK|jHtj|j_t |dr|j |j_yyyw)zmHandle error messages from Edge Proxy. Args: err: ErrorMessage from Edge Proxy. Nmessage)rnrr(r3rrr8)rxerrs r,rz!ActionExecutor._handle_edge_errorsL    ('/D   $sI&*-++!!'' )sAAcKt|dr+|jDcic]}|j|c}|_yycc}ww)z{Handle waypoint list updates from Edge Proxy. Args: msg: WaypointListMessage from Edge Proxy. waypointsN)rrnamerp)rxrwps r,rz$ActionExecutor._update_waypoint_list"s8 3 $58]]Crrww{CDO %CsA;Ac @K|j|}|jjdd}i}g} t|jj } t |j} | r| jd} |j| } |jr!| j|jxsdnb|r||  |j| ||d{}t| | jddd|}||| <| j!| |r|||k|j$dk(r\|j&d k(rM|j)|j*\}}|r-|d |j,|j&||j*|d t |j.j| tD]\}|| vr|j0j|t}|j3|j sL| j|^| r| r| s|js| jd| D]3} |j| } t| | jdddd || <5| xr |j }t5|||| S7#t$rw}t|}t| | jddd | }| j|||| <| j!| |r|||j"rYd}~Yd}~d}~wwxYww)NrG unknown_planrexecution_cancelledr?r_rI)r>r?r3rCr)r>r?r3r8 SCAN_AREAdynamic_replan_requested)typer>r?reasons scan_outputrGdeadlock_detectedskipped)rGrIrKrL)_ensure_plan_graphplangetset step_lookupkeyssorted entry_stepspoprkrrl _execute_stepr< Exceptionr/discardrgr3r?_should_replan_after_scanrCr>children dependenciesissubsetrF)rxrcontext on_step_starton_step_completeon_replan_requested plan_graphrGrKrLpendingreadyr>steprCoutcomeexcrrrchilddepsrIs r, execute_planzActionExecutor.execute_plan,s,,T2 //%%i@*,j,,1134!*"8"89iilG))'2D d11J5JKd# #11$II%#88Hb1$! . 'GG  OOG $ )$/NNi/NNk1#'#A#A'..#Q '$>'.&-nn'.+2>>'.   3 3 7 7 GH ('!..225#%@==0LL'  (yF 6$// MM- . G))'2D*xx"-   GG  *4T__!4wY_``IJ c(%#88Hb1#!   g&#* (#$W-%% sQCLJJ#JDLLA:LJ L$A'L LLLc`g}|jdr|jdt|jddj}t |j Dchc] }||vs| c}}|r#|jddj |t|dkD|fScc}w)zDecide whether SCAN_AREA findings warrant a dynamic replan. Trigger when: - Structured signal says scene is interesting, OR - Free-form analysis text contains hazard/casualty keywords. interestingscan_marked_interestinganalysisr_zanalysis_keywords:,r)rrr/lowerr_SCAN_REPLAN_KEYWORDSjoinlen)rxrCrrkwmatcheds r,rz(ActionExecutor._should_replan_after_scans  ::m $ NN4 5vzz*b1288:t'A'ATR8^"TU  NN/0A/BC DG q 7** Us ! B++B+cK|jd}|jj|}|td|d||||}tj|r |d{S|S7w)Nr?z"No handler registered for action 'z'.)rrir;inspect isawaitable)rxrrrKr?rresponses r,rzActionExecutor._execute_stepss (#..$$V, ?&)KF8SU'VW W4'2   x (!> !"sA"A-$A+%A-ct|tr|S|jj|}|jr |j s/|j r|j jnd}t||j S)NzUnknown parse failure.) isinstancerrcparseokrr8rr;)rxr parse_resultrs r,rz!ActionExecutor._ensure_plan_graphsg dI &K$(KK$5$5d$; l&=&=4@4F4Fl((00LdG&w/ /&&&r+c |j|j|j|j|j|j |j |jdS)N) CHECK_BATTERYNAVIGATEr VERIFY_OBJECTSPEAKWAITIDENTIFY_PERSONALERT_OPERATOR)_handle_check_battery_handle_navigate_handle_scan_area_handle_verify_object _handle_speak _handle_wait_handle_identify_person_handle_alert_operatorrs r,rhz ActionExecutor._default_handlerssT!77--//!77''%%#;;"99  r+cK|jdxsi}|jdd}d}|jr|jjr\|jjF|jjjj }|j jd||t|dd}|d}||k\|dSw) Nparams min_levelrz,CHECK_BATTERY: using live battery level %d%% battery_level)defaultd) battery_ok current_level) rrm is_connectedlast_robot_statebatterylevelrfdebug_context_value)rxrr_resultsrrrs r,rz$ActionExecutor._handle_check_batterys (#)rJJ{A.  $    )!!..!!22>%%66>>DDE KK  Le T ="7OTJE =E#y05IIsCCc@K|jdxsi}|jdd}|jdd}|jdd}|j<|jr||jvrd||dd S|j|||d{Sd ||d S7 w) Nrr1unknownr2normalr>runknown_waypointr3r1r2r8rr3r1r2)rrmrp_navigate_with_edge_proxy)rxr_contextrrr1r2r>s r,rzActionExecutor._handle_navigates (#)rjj :  7H-((9i0    (;doo#E&#."/  77 UGTT T$K%PPUsBBB BcDKddlm}d|}t|||tj|_ |j j|||d{|jd{ |j|d d{ |j'd{d||dS7W#t$r6}|jjd|d||t|d cYd}~Sd}~wwxYw77i#tj$r|jjd |tj |j _d |j _ |j%d{7d||d d cY|j'd{7St$rh}|jjd||j%d{7d||t|d cYd}~|j'd{7Sd}~wwxYw7`#|j'd{7wxYww)a Execute navigation using Edge Proxy client. Args: destination: Target waypoint name. speed: Navigation speed. step_id: Step ID for tracking. Returns: Result dictionary with navigation outcome. r) NavStatusnav_)r0r1r2r3)rr0r2Nz#Failed to send navigate command: %srr^@timeoutzNavigation timeout for %sr!zNavigation failed: %srr)edge_proxy.messagesrr.rr%rnrmsend_navigate_waypointrrfr8r/r_wait_for_navigationasyncio TimeoutErrorr(r3rr)rxr1r2r>rr0rs r,rz(ActionExecutor._navigate_with_edge_proxys 2G9% -!#$$   ##:: Zu;  ""$$$ (++J+F F F.%%' ' ' &  W   KK  CS I"*S    % G##  KK  9; G'/D   $&/D   #;;= = ="*"  "%%' ' '  KK  5s ;;;= = ="*S  %%' ' '  ($%%' ' 's.H !B,B*B,H *C.+H 0C2C0C2 H G? H *B,, C+5+C& C+!H &C++H 0C22A2G<$E'% G<1H2H FH  G</G7GG7G<HH 0G31H 7G<<H?H HHHH cKtjj}d} tjj|z }||kDrtjd|d||j t d|jj }|tjk(r|jdd{y|tjk(r(|jjxsd}td||tjk(r td |jjd kDr-|j|jjd{tj|d{]77%7 w) a&Wait for navigation to complete or fail. Args: request_id: Navigation request ID to track. timeout: Maximum time to wait in seconds. Raises: asyncio.TimeoutError: If navigation times out. Exception: If navigation fails. 皙?zNavigation timeout after zs for request Nz#Navigation context lost during waitrzUnknown navigation failurezNavigation failed: zNavigation was cancelledr)r%get_event_looptimer&rn RuntimeErrorr3rr'rr(r8rr)r6sleep)rxr0r! start_timecheck_intervalelapsedr3r8s r,r$z#ActionExecutor._wait_for_navigationIso++-224 ,,.335 BG **/yzlS   ("#HII&&--F)))33C8888??*))//O3O"5eW =>>8--- :;;  ))A-33D4E4E4N4NOOO--/ / /; 9P 0s7B?F FBF !F"F >F ?F F  F c(K|jdxsi}|jdd}d}|jd{}||jjd|d}nEt j } | j d|jj||d{}|j} t|jD chc] } | | vs|  c} } t| d kD}|j |jd |d d{||d || dS77}#t$r(} |jjd| d}Yd} ~ d} ~ wwxYwcc} w7I#t$r&} |jjd | Yd} ~ qd} ~ wwxYww)aRun VLM scene analysis on the current camera frame. Captures a JPEG frame, sends it to the VLM endpoint together with a chemical-spill-oriented prompt, broadcasts the result to the dashboard as a ``vlm_caption`` message, and returns a structured outcome dict. If no frame is available (camera not wired), the VLM call is skipped and the analysis field contains an empty string. rtargetareau;You are an emergency-response robot assistant analysing a scene for a chemical spill exercise. Describe what you see concisely: number and state of any people (conscious / unconscious), visible chemical containers or hazard markings, exit routes, and any immediate dangers. Be factual and brief (3–5 sentences).NuASCAN_AREA: no frame available — skipping VLM call for target %rr_zSCAN_AREA: VLM call failed: %sr vlm_caption)rtextz.SCAN_AREA: failed to broadcast vlm_caption: %sgY@)rr1coverage_percentrsignals)r_capture_framerfwarningr%r)run_in_executorrv analyze_scenerr8rrrrrw)rxrrrrr1promptframerloopr analysis_lcrr6rs r,rz ActionExecutor._handle_scan_areaxs(#)rHf- J (,':':' target_lcrBs r,rz$ActionExecutor._handle_verify_objectsU(#)r::.9Hh/==CHEP Q (,':':',AD4 (D4D5D9D4D D1 D,'D4,D11D4cK|jdxsi}|jdd}tjt|ddz d{d|iS7w)Nr duration_msrg@@ waited_ms)rr%r,max)rxrrrrrWs r,rzActionExecutor._handle_waitsY (#)rjj2 mmC Q/&8999[)) :sA AA AcK|jd{}|!|jjdddddS|jj |d{}|s!|jj dddgdSt |d }|jj d |jd |jd d|jd d|jd d|jd |dS77w)Nu9IDENTIFY_PERSON: no frame available — returning unknownrr4no_frame)identityrDr8z*IDENTIFY_PERSON: FR returned no detections)r\rD detectionsc&|jddS)NrDr4)r)ds r,z8ActionExecutor._handle_identify_person..0sQUU<-Er+)keyz7IDENTIFY_PERSON: best match identity=%r confidence=%.2fr\rDbbox)r\rDrbr])r7rfr8ruidentifyinforYr)rxrrr frame_bytesr]bests r,rz&ActionExecutor._handle_identify_persons!//11   KK   [ \ )zR R ??33K@@  KK  I J )BO O:#EF  E HHZ HH\3 ' Y7((<5HHV$$   '2 As#DDADDB(DDcK|j|jjs|jjdyt t j }tj}|j}||jj|< |jj|d{ ddlm}tj"||d{}|jr'|jj'd |jy|j(s|jj'd y t+j,|j(}|jjd t/||S7#t$rL}|jjd||jjj|dYd}~yd}~wwxYw7#tj$$rE|jj'd||jjj|dYyt$rL}|jjd ||jjj|dYd}~yd}~wwxYw#t$r&}|jjd |Yd}~yd}~wwxYww)aCapture a single JPEG frame from the robot camera via the Edge Proxy. Sends a ``capture_frame`` command over the existing Edge Proxy WebSocket connection and waits for the matching ``frame_response`` message. The Edge Proxy handles the actual frame grab (HTTP snapshot from MediaMTX, with an ffmpeg-RTSP fallback) so this method only needs the WS connection. Args: timeout: Maximum seconds to wait for the frame response. Returns: Raw JPEG bytes decoded from the base64 response, or ``None`` if no Edge Proxy client is connected, capture fails, or the request times out. NuA_capture_frame: no connected edge proxy client — returning None)r0z0_capture_frame: failed to send capture_frame: %sr)FrameResponseMessager z@_capture_frame: timed out after %.1fs waiting for frame_responsez4_capture_frame: error waiting for frame_response: %sz-_capture_frame: edge proxy reported error: %sz/_capture_frame: frame_response missing jpeg_b64z+_capture_frame: failed to decode base64: %sz!_capture_frame: received %d bytes)rmr rfrr/uuiduuid4r%r) create_future_pending_framessend_capture_framerr8rr"rhwait_forr&r8jpeg_b64rR b64decoder) rxr!r0r=futurerrhrres r,r7zActionExecutor._capture_frame=sA$    $D,=,=,J,J KK  a b& %%'!%!3!3!58>))*5 ##66*6M M M  @.5.>.>vw.W(WC 99 KK   OQTQZQZ [|| KK   Q R  **3<<8K =s;?OPG N  KK  PRU V    - - 1 1*d C )X##  KK  RT[     - - 1 1*d C  KK  TVY Z    - - 1 1*d C   KK  KS Q sBKF ;F<F  G%!G""G%&AKJ!'KF GAGKGK"G%%AJ:K<JAJ K JK KJ=8K=KKcK|jdxsi}|jdd}|jdd}|jQddl}d|||jd} |j|d{|jj d ||n|jj d d ||d S7C#t $r&} |jj d | Yd} ~ 2d} ~ wwxYww)Nrr unspecifiedseverityrdralert)rrrtrPz'Alert broadcasted to dashboard: %s (%s)z*Failed to broadcast alert to dashboard: %szMALERT_OPERATOR: no broadcast callback registered, alert not sent to dashboardT)alertedrrt)rrwr*rfrdrr8) rxrrrrrrtr* alert_messagers r,rz%ActionExecutor._handle_alert_operators (#)rHm4::j&1  # # /   $!YY[ -M  W..}===   !JFT\] KK   o p   > W ##$PRUVV WsBAC."B<6B:7!B<"C.:B<< C+C&!C.&C++C.)NNNT) ryz"Optional[Dict[str, ActionHandler]]rczOptional[DAGParser]rfzOptional[logging.Logger]rgrHreturnNone)r|z5Optional[Callable[[Dict[str, Any]], Awaitable[None]]]rxry)r?r/r ActionHandlerrxryr)rr7rxry)rxry)r Optional[Any]rxry)rrrxry)r6r5rxry)rrrxry)rrrxry)rrrxry)NNNN) r%Union[PlanGraph, Dict[str, Any], str]rr{rz*Optional[Callable[[Dict[str, Any]], None]]rz'Optional[Callable[[StepOutcome], None]]rzOptional[ReplanCallback]rxrF)rCrBrxztuple[bool, List[str]])rrBrr{rKrJrxrB)rr|rxr)rxzDict[str, ActionHandler])rrBrr{rrJrxrB)rrBrr{rrJrxrB)r1r/r2r/r>r/rxrB)r)r0r/r!r5rxry)g @)r!r5rxzOptional[bytes])$r!r"r#rrzr}rrrrrrrrrrrrrrrrrrrhrrrr$rrrrrr7rr*r+r,rOrOQs&8<&*+/" /_4/_$/_) /_  /_  /_b ,M ,  ,)%#D&'   .-$5L2 6D"&DHDH8< aa3aaaaB aa B aa 6 aa aaF+&(    '  JJJ) J  J:QQ Q) Q  Q4I I '*I 58I I X16-0-0(--0 -0^= =  = ) =  = ~3 3  3 ) 3  3 j#4#4 #4) #4  #4J * *  *) *  *    )    BAF    )    r+rOch||St|tr|j||St|||Sr)rrDrr)rrar s r,rrs6'4 {{3(( 7C ))r+r)rr{rar/r rrxr)1 __future__rr%rRrrri dataclassesrrenumrrrdtypingrrr r r r r rplanner.dag_parserrrrexecutor.triggersrrservices.tts_clientrrservices.fr_clientrrservices.vlm_clientrrr/rr.r+r;rzReplanCallbackr<rFrOrr*r+r,rs!" (RRR@@?414 sD         <  #s(^Xc]Dm);$<= $sCx.)DcN3 346 4S>*D01      44 4N  N  b*r+