
    SiO#                        d dl mZ d dlmZ d dlmZmZmZmZm	Z	 ddl
mZ h dZddhZe G d	 d
             Ze G d d             Z G d d      Zy)    )annotations)	dataclass)AnyDictListOptionalSet   )	PlanGraph>   WAITSPEAKNAVIGATE	SCAN_AREACHECK_BATTERYVERIFY_OBJECTALERT_OPERATORIDENTIFY_PERSONCRITICALHIGHc                  @    e Zd ZU ded<   ded<   ded<   ded<   ded<   y)		Violationstrrule_idseveritymessage	List[str]affected_stepssuggested_fixN__name__
__module____qualname____annotations__     G/home/nelsen/Projects/HRI/orchestrator/src/planner/symbolic_verifier.pyr   r      s    LMLr%   r   c                  ,    e Zd ZU ded<   ded<   ded<   y)VerificationResultboolpassedzList[Violation]
violationsr   warningsNr   r$   r%   r&   r(   r(       s    Lr%   r(   c                  d    e Zd Zd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y)SymbolicVerifierc                    || _         y N)max_plan_depth)selfr1   s     r&   __init__zSymbolicVerifier.__init__(   s
    ,r%   Nc                "   g }g }| j                  |      }|j                  j                         D ]B  \  }}|j                  d      }|s||vs|j	                  t        ddd| d|gd             D | j                  |      }	|	r|j	                  t        dd	d
|	d             |j                  sA|j	                  t        dd	dt        |j                  j                               d             n;| j                  |      }
|
r(|j	                  t        dd	dt        |
      d             |j                  j                         D ]  \  }}|j                  d      dk(  s| j                  ||d       }|s |j	                  t        dd	d|gd             |j                  d      xs i }|j                  d      dk(  s{| j                  ||d       }|r|j	                  t        dd	d|gd              |	sq| j                  |      }|| j                  kD  rQ|j	                  t        ddd| d| j                   d t        |j                  j                               d!             t        d" |D               }t!        |||#      S )$Nactionunknown_actionr   zAction "z" is not in allowed actions.z(Use only allowed actions for this robot.)r   r   r   r   r   	no_cyclesr   zPlan contains a cycle.z+Remove circular dependencies between steps.no_orphan_nodeszPlan has no entry steps.z-Ensure at least one step has no dependencies.z0Some steps are unreachable from the entry point.z#Connect all steps to the main flow.r   c                *    | j                  d      dk(  S )Nr5   r   getss    r&   <lambda>z)SymbolicVerifier.verify.<locals>.<lambda>d   s    155?o3U r%   battery_before_movementz+NAVIGATE requires CHECK_BATTERY beforehand.z"Add CHECK_BATTERY before NAVIGATE.paramsspeedfastc                |    | j                  d      dk(  xr' | j                  d      xs i j                  d      dk(  S )Nr5   r   r@   targethumansr:   r<   s    r&   r>   z)SymbolicVerifier.verify.<locals>.<lambda>v   s<    !%%/["@ #NUU8_277AXM r%   human_proximity_before_speedz2Fast NAVIGATE requires SCAN_AREA targeting humans.z2Add SCAN_AREA(target=humans) before fast NAVIGATE.r1   zPlan depth z exceeds limit .z0Shorten the plan or split it into smaller plans.c              3  @   K   | ]  }|j                   t        v   y wr0   )r   BLOCKING_SEVERITIES).0vs     r&   	<genexpr>z*SymbolicVerifier.verify.<locals>.<genexpr>   s     Oq'::Os   )r*   r+   r,   )_resolve_allowed_actionsstep_lookupitemsr;   appendr   _detect_cycleentry_stepslistkeys_find_unreachablesorted_has_ancestor_longest_path_lengthr1   anyr(   )r2   
plan_graphcontextr+   r,   allowed_actionsstep_idstepr5   cycle_detectedunreachablehas_batteryr@   has_scan	max_depthr*   s                   r&   verifyzSymbolicVerifier.verify+   s   &(
 77@'3399; 	MGTXXh'F6#@!! 0!'"*6(2N O(/y&P	 ++J7''4#1"O %%-'6#'
(>(>(C(C(E#F"Q 00<K!! 1!+ R'-k':&K (3399; !	MGTxx!Z/"00)U #%%!$=%/$Q,39*N (+1r::g&&0#11"N H $"))%(F)3(\07y.b3!	F 11*=I4...!! 0!'"-i[H[H[G\\] ^'+J,B,B,G,G,I'J&X OJOOO!JQYZZr%   c                    |j                   ryddg}|j                  D ]  }|j                  d|j                   d|j                   d       |j                  d|j
                          |j                  r-|j                  dd	j                  |j                                |j                  r|j                  d
|j                          |j                  d        dj                  |      j                         S )NzPlan accepted.z Your previous plan was REJECTED. zVIOLATION: z ()z	DETAILS: z
AFFECTED: z, zFIX: 
)
r*   r+   rP   r   r   r   r   joinr   strip)r2   resultlines	violations       r&   format_feedbackz SymbolicVerifier.format_feedback   s    ==#3R8** 	ILL;y'8'8&9I<N<N;OqQRLL9Y%6%6$789''z$))I4L4L*M)NOP&&uY%<%<$=>?LL	 yy%%''r%   c                    |t        t              S t        |dd       }|rt        |      S t        |t              r|j                  d      rt        |d         S t        t              S )Navailable_actions)setDEFAULT_ALLOWED_ACTIONSgetattr
isinstancedictr;   )r2   r[   actionss      r&   rM   z)SymbolicVerifier._resolve_allowed_actions   sb    ?.//'#6=w<gt$5H)Iw2344*++r%   c                z   t        |j                  j                  |t                           }t               }|r|j	                         }||v r|j                  |       |j                  j                  |      }|r	 ||      ry|j                  |j                  j                  |t                            |ry)NTF)rS   dependenciesr;   rq   popaddrN   extend)r2   rZ   r]   	predicatestackvisitedcurrentr^   s           r&   rW   zSymbolicVerifier._has_ancestor   s     Z,,00#%@AEiikG'!KK ))--g6D	$LL0044WceDE  r%   c                L   t               }t        |j                        }|r\|j                         }||v r|j	                  |       |j                  |j                  j                  |t                            |r\t        |j                  j                               }||z
  S r0   )
rq   rS   rR   ry   rz   r{   childrenr;   rN   rT   )r2   rZ   	reachabler}   r   	all_stepss         r&   rU   z"SymbolicVerifier._find_unreachable   s    !e	Z++,iikG)#MM'"LL,,00#%@A  
..3356	9$$r%   c                r    | j                  |      }|#t        |j                  j                               S g S r0   )_topological_sortrS   rN   rT   )r2   rZ   orders      r&   rQ   zSymbolicVerifier._detect_cycle   s6    &&z2=
..33566	r%   c                8   i }|j                   j                         D ]2  }t        |j                  j	                  |t                           ||<   4 |j                         D cg c]  \  }}|dk(  s| }}}g }|rt|j                         }|j                  |       |j                  j	                  |t                     D ])  }||xx   dz  cc<   ||   dk(  s|j                  |       + |rtt        |      t        |j                         k7  ry |S c c}}w )Nr   r
   )
rN   rT   lenrx   r;   rq   rO   ry   rP   r   )	r2   rZ   	in_degreer]   degreequeuer   nodechilds	            r&   r   z"SymbolicVerifier._topological_sort   s	   $&	!--224 	RG!$Z%<%<%@%@#%%P!QIg	R 1:0AQ_WfVq[QQ99;DLL#,,00su= (% A% U#q(LL'(  u:Z3344 Rs   %D3Dc                f   | j                  |      }|| j                  dz   S |j                  j                         D ci c]  }|d }}|D ]C  }|j                  j                  |t                     D ]  }t        ||   ||   dz         ||<    E t        |j                         d      S c c}w )Nr
   r   )default)	r   r1   rN   rT   r   r;   rq   maxvalues)r2   rZ   r   r]   	distancesr   r   s          r&   rX   z%SymbolicVerifier._longest_path_length   s    &&z2=&&**?I?U?U?Z?Z?\$]GWaZ$]	$] 	ND#,,00su= N#&y'7419L#M	% N	N 9##%q11	 %^s   
B.)   )r1   intreturnNoner0   )rZ   r   r[   Optional[Any]r   r(   )rk   r(   r   r   )r[   r   r   Set[str])rZ   r   r]   r   r   r)   )rZ   r   r   r   )rZ   r   r   r   )rZ   r   r   zOptional[List[str]])rZ   r   r   r   )r    r!   r"   r3   rd   rn   rM   rW   rU   rQ   r   rX   r$   r%   r&   r.   r.   '   sN    -g[R( , 
 
&
%(	2r%   r.   N)
__future__r   dataclassesr   typingr   r   r   r   r	   
dag_parserr   rr   rI   r   r(   r.   r$   r%   r&   <module>r      sg    " ! 1 1 !	  "6*       L2 L2r%   