
    Gi3                     h   d dl Zd dlmc 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mZ  G d d      Zd Zddededee   fd	Ze	j&                  j                  d
        Ze	j&                  j                  d        Ze	j&                  j                  d        Ze	j&                  j                  d        Zy)    N)EdgeProxyServerc                   $    e Zd Zdedz  fdZd Zy)_FakeRosbridgeposeNc                .    || _         d| _        g | _        y )NF)r   	pose_sent	published)selfr   s     U/home/nelsen/Projects/kognitive/edge-proxy/tests/test_server_rosbridge_integration.py__init__z_FakeRosbridge.__init__   s    	%'    c           
        K   |2 3 d {   }t        j                  |      }|j                  d      }|dk(  rp|j                  d      }|dk(  rY| j                  M| j                  sAd| _        |j                  t        j                  dd| j                  d             d {    |dk7  r| j                  j                  |       |j                  d      dk7  r|j                  t        j                  dd	d
did             d {    |j                  t        j                  ddddigddd             d {    |j                  t        j                  dd	d
did             d {    v7 r7 7 s7 ?7 6 y w)Nop	subscribetopiczgui/get_robot_poseTpublish)r   r   msggui/execute_planzgui/get_plan_feedbackdata   zgui/get_waypoints_newstatus   r   )	waypointsnext_waypoint)	jsonloadsgetr   r   senddumpsr	   append)r
   wsrawr   r   r   s         r   handlerz_FakeRosbridge.handler   s     7	 7	#**S/CB[ (00TYY5JSWSaSa%)DN''

&/)='+yy   YNN!!#&www#55''

'!8 &{   ''

'!8+3Q--. 	   ''

'!8 &{  _7	$_ so   FFE;FBF#E>$A(FF 5FF1F4F5F;F>F FFFF)__name__
__module____qualname__dictr   r#    r   r   r   r      s    (t (
8r   r   c                   K   t        j                  | j                          d {         }t        j                  | j                          d {         }|d   }d}||v }|slt        j                  d|fd||f      t        j
                  |      t        j
                  |      dz  }dd|iz  }t        t        j                  |            d x}x}}|d   }d}||v }|slt        j                  d|fd||f      t        j
                  |      t        j
                  |      dz  }dd|iz  }t        t        j                  |            d x}x}}y 7 37 w)Ntype)waypoint_listrobot_statein)z%(py1)s in %(py4)spy1py4assert %(py6)spy6)r   r   recv
@pytest_ar_call_reprcompare	_safereprAssertionError_format_explanation)r!   msg1msg2@py_assert0@py_assert3@py_assert2@py_format5@py_format7s           r   _drain_initialrA   L   s     ::BGGIo&D::BGGIo&D<;;;<;;;;;<;;;;<;;;;;;;;;;;<;;;<;;;;;<;;;;<;;;;;;;;;;; &%s"   #EE*EEDEE
request_idtimeout_secreturnc                    K   g }t        j                         j                         |z   }t        j                         j                         |k  rt        j                  | j                          d {         }|j                  d      dk7  s|j                  d      |k7  ry|j                  |j                  d             |j                  d      dv r|S t        j                         j                         |k  rt        d|       7 w)Nr*   
nav_statusrB   r   >   failedarrived	cancelledz.Timed out waiting for terminal nav status for )	asyncioget_event_looptimer   r   r4   r   r    r8   )r!   rB   rC   statusesdeadliner   s         r   _collect_nav_statusesrO   S   s     H%%',,.<H

 
 
"
'
'
)H
4jjrwwy)776?l*cggl.Cz.Q)*778 BBO 
 
 
"
'
'
)H
4 I*V
WW )s   A/D1D2BD>Dc                 
  K   | dz  }|j                  dj                                t        dddd      }t        j                  |j
                  dd       d {   }	 |j                  d   j                         d   }t        ddd	d
dt        |      d| d      }|j                  j                          d {    t        j                  |j                  |j                  d|j                         d {   }	 |j                  d   j                         d   }t        j                  d| d	      4 d {   }t!        |       d {    dD ]  \  }	}
|j#                  t%        j&                  d|	d|
ddd             d {    t)        ||	       d {   }d}||v }|st+        j,                  d|fd||f      t+        j.                  |      dt1        j2                         v st+        j4                  |      rt+        j.                  |      nddz  }dd|iz  }t7        t+        j8                  |            d x}}d}||v }|st+        j,                  d|fd||f      t+        j.                  |      dt1        j2                         v st+        j4                  |      rt+        j.                  |      nddz  }dd|iz  }t7        t+        j8                  |            d x}} d d d       d {    |j:                  D cg c]  }|j=                  d      d k(  s| }}t?        |      }d!}||k(  }|st+        j,                  d"|fd#||f      d$t1        j2                         v st+        j4                  t>              rt+        j.                  t>              nd$d%t1        j2                         v st+        j4                  |      rt+        j.                  |      nd%t+        j.                  |      t+        j.                  |      d&z  }d'd(|iz  }t7        t+        j8                  |            d x}x}}|jA                          |jC                          d {    |j                  jE                          d {    	 |jA                          |jC                          d {    y 7 ]7 7 7 7 r7 87 &7 # 1 d {  7  sw Y   xY wc c}w 7 7 c# |jA                          |jC                          d {  7   |j                  jE                          d {  7   w xY w7 # |jA                          |jC                          d {  7   w xY ww))Nwps.yamlz
waypoints:
  - name: office_scene
    x: 1.0
    y: 1.0
    theta: 0.0
  - name: boxes_scene
    x: 2.0
    y: 3.0
    theta: 0.0
  - name: spill_scene
    x: 5.0
    y: 2.0
    theta: 0.0
        xyzr   	127.0.0.1r   r   /edge/health	rosbridgews://127.0.0.1:Fhostportws_pathhealth_pathbackendwaypoints_pathrosbridge_urlrosbridge_insecure_tlsprocess_request))officeoffice_scene)boxesboxes_scene)spillspill_scenenavigatewaypointr*   namenormalr*   rB   goalspeedrB   acceptedr-   z%(py1)s in %(py3)srM   r0   py3assert %(py5)spy5rH   r   r      ==)z0%(py3)s
{%(py3)s = %(py0)s(%(py1)s)
} == %(py6)slenexecute_msgs)py0r0   rz   r3   zassert %(py8)spy8)#
write_textlstripr   
websocketsserver#   socketsgetsocknamer   str_backendstart_handlerr^   _process_requestconnectrA   r   r   r   rO   r5   r6   r7   @py_builtinslocals_should_repr_global_namer8   r9   r	   r   r   closewait_closedstop)tmp_pathr   fake_rb	rb_serverrb_portserveredge_ws_server	edge_portr!   rB   rq   rM   r<   r>   @py_format4@py_format6mr   @py_assert5@py_assert4r@   @py_format9s                         r   2test_rosbridge_demo_scenario_three_scene_waypointsr   `   s    :%I	 FH$ #C"@AG &&wQGGI2&##A&224Q7 !y>+G95#(	
 oo##%%%)//OOKK"33	 
 
	)&..q1==?BI!))OI;e*LM 1 1QS$R((() 1$J
 ''

(2.81;T(J)1		 	 	 &;2*%UUH%1:1111:111:1111111111111111$090000900090000000000000000#11 1* (/'8'8a!AEE'NN`<`AaLa|$))$))))$))))))3)))3))))))|)))|)))$))))))))))  " ,,...//&&(((##%%%g H 	&
1(	  V#1 1 1 1* b /(   " ,,...//&&((( 	& 	##%%%sS  AU0R8U0!AU =R;>>U <R>=U =S/ ?S S/ SS<SSS$S
%ES6S/ SS/ S&.S&2DS/ #U 'S+(!U 	S-
U #U02U 3U0;U >U S/ SS
SS/ S#SS#S/ +U -U /$T=T"T=6T97T==U  U0$U-&U)'U--U0c                 r  K   | dz  }|j                  d       t        ddt        j                  dz  d      }t	        j
                  |j                  dd	       d {   }	 |j                  d	   j                         d
   }t        dd	dddt        |      d| d      }|j                  j                          d {    t	        j
                  |j                  |j                  d	|j                         d {   }	 |j                  d	   j                         d
   }t	        j                   d| d      4 d {   }t#        |       d {    t%        j&                  d       d {    |j)                  t+        j,                  dddddddd             d {    t/        |d       d {   }	d}
|
|	v }|st1        j2                  d|fd|
|	f      t1        j4                  |
      dt7        j8                         v st1        j:                  |	      rt1        j4                  |	      ndd z  }d!d"|iz  }t=        t1        j>                  |            d x}
}d d d       d {    |j@                  D cg c]  }|jC                  d#      d$k(  s| }}|s{t1        jD                  d%      d&z   d'd(t7        j8                         v st1        j:                  |      rt1        j4                  |      nd(iz  }t=        t1        j>                  |            |d)   d*   }|d+   d,   }
d-}|
|k(  }|slt1        j2                  d.|fd/|
|f      t1        j4                  |
      t1        j4                  |      d0z  }d1d2|iz  }t=        t1        j>                  |            d x}
x}}|d3   d	   d4   d5   }|d6   }
tF        jH                  }d}d7} |||8      }|
|k(  }|st1        j2                  d.|fd9|
|f      t1        j4                  |
      d:t7        j8                         v st1        j:                  tF              rt1        j4                  tF              nd:t1        j4                  |      t1        j4                  |      t1        j4                  |      t1        j4                  |      d;z  }d<d=|iz  }t=        t1        j>                  |            d x}
x}x}x}x}}|d>   }
tF        jH                  }d?}d7} |||8      }|
|k(  }|st1        j2                  d.|fd9|
|f      t1        j4                  |
      d:t7        j8                         v st1        j:                  tF              rt1        j4                  tF              nd:t1        j4                  |      t1        j4                  |      t1        j4                  |      t1        j4                  |      d;z  }d<d=|iz  }t=        t1        j>                  |            d x}
x}x}x}x}}|jK                          |jM                          d {    |j                  jO                          d {    	 |jK                          |jM                          d {    y 7 7 7 7 C7 37 7 7 7 ## 1 d {  7  sw Y   4xY wc c}w 7 7 f# |jK                          |jM                          d {  7   |j                  jO                          d {  7   w xY w7 # |jK                          |jM                          d {  7   w xY ww)@NrQ   waypoints: []
g      $@      @r   rS   rW   rX   r   r   rY   rZ   r[   r\   Fr]   rf   皙?rn   relative_forwardrelativeforward       @r*   	directiondistanceslowrs   rv   rH   r-   rx   rM   ry   r{   r|   r   zgui/add_waypoint_newz-Expected gui/add_waypoint_new to be publishedz
>assert %(py0)sr   add_msgsr   headerframe_idmapr~   )z%(py1)s == %(py4)sr/   r2   r3   r   r   positionrT   gMbP?)abs)z[%(py1)s == %(py11)s
{%(py11)s = %(py5)s
{%(py5)s = %(py3)s.approx
}(%(py7)s, abs=%(py9)s)
}pytest)r0   rz   r|   py7py9py11zassert %(py13)spy13rU   g      @)(r   r   mathpir   r   r#   r   r   r   r   r   r   r   r^   r   r   rA   rJ   sleepr   r   r   rO   r5   r6   r7   r   r   r   r8   r9   r	   r   _format_assertmsgr   approxr   r   r   )r   r   r   r   r   r   r   r   r!   rM   r<   r>   r   r   r   r   @py_format1waypoint_msgr=   r?   r@   r   r   @py_assert6@py_assert8@py_assert10@py_format12@py_format14s                               r   6test_rosbridge_relative_goal_resolves_to_absolute_poser      sD    :%I*+3TWWq["IJG &&wQGGI7&##A&224Q7 !y>+G95#(	
 oo##%%%)//OOKK"33	 
 
!	)&..q1==?BI!))OI;e*LM - -QS$R(((mmD)))ggJJ$.*<(2-6,/%
 &,	   "7rFX!YY ,yH,,,,yH,,,y,,,,,,H,,,H,,,,,,,%- -( $+#4#4aagJ`8`aHaLLLLLLLLL8LLL8LLLLL#B<.L)*5>>5>>>>5>>>5>>>>>>>>>>,Q/7
CD9==d==d ==9 =====9 ====9===============d====== =========9<<c<t<ct <<9 <<<<<9 <<<<9<<<<<<<<<<<<<<<c<<<t<<< <<<<<<<<<  " ,,...//&&(((##%%%q H 	&
-() Z#- - - -( b /(   " ,,...//&&((( 	& 	##%%%se  A\7Y<\7$A\	  Y?>\	 ?Z \	 =Z6 ZZ6 ZZZ2Z34Z'Z(Z<Z=B)Z&Z6 1Z2Z6 Z-Z-"M&Z6 #\	 +Z2,!\	 Z4\	 #\76\7\7?\	 \	 Z6 ZZZZZ6 Z*Z Z*%Z6 2\	 4\	 6$\["\=\ >\\	 \7	$\4-\0.\44\7c                 D  K   | dz  }|j                  d       t        d       }t        j                  |j                  dd       d {   }	 |j
                  d   j                         d   }t        ddddd	t        |      d
| d      }|j                  j                          d {    t        j                  |j                  |j                  d|j                         d {   }	 |j
                  d   j                         d   }t        j                  d
| d      4 d {   }t        |       d {    |j!                  t#        j$                  dddddddd             d {    t'        |d       d {   }	d}
|
|	v }|st)        j*                  d|fd|
|	f      t)        j,                  |
      dt/        j0                         v st)        j2                  |	      rt)        j,                  |	      nddz  }dd|iz  }t5        t)        j6                  |            d x}
}d d d       d {    |j8                  D cg c]  }|j;                  d      dk(  s| }}| }|s{t)        j<                  d       d!z   d"d#t/        j0                         v st)        j2                  |      rt)        j,                  |      nd#iz  }t5        t)        j6                  |            d }|j?                          |jA                          d {    |j                  jC                          d {    	 |j?                          |jA                          d {    y 7 7 7 7 D7 47 7 7 ># 1 d {  7  sw Y   OxY wc c}w 7 7 c# |j?                          |jA                          d {  7   |j                  jC                          d {  7   w xY w7 # |j?                          |jA                          d {  7   w xY ww)$NrQ   r   rW   rX   r   r   rY   rZ   r[   r\   Fr]   rf   rn   relative_failr   leftg      ?r   r   rs   rv   rG   r-   rx   rM   ry   r{   r|   r   r   z9No execute_plan publish expected when pose is unavailablez
>assert not %(py0)sr   r   )"r   r   r   r   r#   r   r   r   r   r   r   r   r^   r   r   rA   r   r   r   rO   r5   r6   r7   r   r   r   r8   r9   r	   r   r   r   r   r   )r   r   r   r   r   r   r   r   r!   rM   r<   r>   r   r   r   r   @py_assert1@py_format2s                     r   8test_rosbridge_relative_goal_fails_when_pose_unavailabler      sP    :%I*+$'G &&wQGGI1&##A&224Q7 !y>+G95#(	
 oo##%%%)//OOKK"33	 
 
	)&..q1==?BI!))OI;e*LM , ,QS$R(((ggJJ$.*9(2-3,/%
 &,	   "7ro!VV+x8++++x8+++x++++++8+++8+++++++%, ,( (/'8'8a!AEE'NN`<`AaLa##`#``%```````|```|``````  " ,,...//&&(((##%%%e H 	&
,( W#, , , ,( b /(   " ,,...//&&((( 	& 	##%%%sS  AP 	M(
P AO2 +M+,>O2 *M.+O2 0=N -M1.N 1N  M44N 5M76N 
M:B)N 4N ?M= N N,N0BN 4#O2 N!O2 9N:O2 ?#P "O0#P +O2 .O2 1N 4N 7N :N =N  NN	NN O2 O2 $O-O"O-&O)'O--O2 0P 2$PPPP c           
      @  K   | dz  }|j                  dj                                t        dddd      }t        j                  |j
                  dd       d {   }	 |j                  d   j                         d   }t        ddd	d
dt        |      d| d      }|j                  j                          d {    t        j                  |j                  |j                  d|j                         d {   }	 |j                  d   j                         d   }t        j                  d| d	      4 d {   }t!        |       d {    |j#                  t%        j&                  ddddddd             d {    d d d       d {    t)        j*                         j-                         dz   }	t)        j*                         j-                         |	k  r`t/        d |j0                  D              rnNt)        j2                  d       d {    t)        j*                         j-                         |	k  r`t5        d      t        j                  d| d	      4 d {   }
t!        |
       d {    d}t)        j*                         j-                         dz   }t)        j*                         j-                         |k  r|
j#                  t%        j&                  ddd             d {    t%        j6                  |
j9                          d {         }|j;                  d       d!k7  r|j;                  d"d      }|d#k(  rnCt)        j2                  d$       d {    t)        j*                         j-                         |k  rd#}||k(  }|st=        j>                  d%|fd&||f      d"tA        jB                         v st=        jD                  |      rt=        jF                  |      nd"t=        jF                  |      d'z  }d(d)|iz  }t5        t=        jH                  |            d x}}d d d       d {    |jK                          |jM                          d {    |j                  jO                          d {    	 |jK                          |jM                          d {    y 7 j7 7 7 7 7 N7 A# 1 d {  7  sw Y   RxY w7 7 7 v7 7 7 7 # 1 d {  7  sw Y   xY w7 7 # |jK                          |jM                          d {  7   |j                  jO                          d {  7   w xY w7 # |jK                          |jM                          d {  7   w xY ww)*NrQ   zH
waypoints:
  - name: office_scene
    x: 1.0
    y: 1.0
    theta: 0.0
rR   rS   rW   rX   r   r   rY   rZ   r[   r\   Fr]   rf   rn   disconnect_casero   ri   rp   rr   rs   r   c              3   D   K   | ]  }|j                  d       dk(    yw)r   r   N)r   ).0r   s     r   	<genexpr>zJtest_navigation_continues_after_orchestrator_disconnect.<locals>.<genexpr>]  s     WquuW~);;Ws    r   z7Edge proxy did not execute plan after client disconnect g      @	get_statestate_probe)r*   rB   r*   r,   	nav_staterH   g?r~   )z%(py0)s == %(py3)s)r   rz   r{   r|   )(r   r   r   r   r   r#   r   r   r   r   r   r   r   r^   r   r   rA   r   r   r   rJ   rK   rL   anyr	   r   r8   r   r4   r   r5   r6   r   r   r   r7   r9   r   r   r   )r   r   r   r   r   r   r   r   r!   execute_deadlinews2r   rN   r   r>   r   r   r   s                     r   7test_navigation_continues_after_orchestrator_disconnectr   (  s    :%I	 FH #C"@AG &&wQGGI@&##A&224Q7 !y>+G95#(	
 oo##%%%)//OOKK"33	 
 
*	)&..q1==?BI!))OI;e*LM  QS$R(((ggJJ$.*;-7$P%-		 	 	   '557<<>D((*//14DDWWEVEVWWmmD))) ((*//14DD
 %%^__!))OI;e*LM . .QT$S)))	"11388:S@,,.335@((4::{R_.`#abbb**388:%56Cwwv-7  #R 8I I-!--,,, ,,.335@ %.-yI----yI------y---y---I-------. .   " ,,...//&&(((##%%%C H 	&
(	   $ *.) c%5 -. . . .  /(   " ,,...//&&((( 	& 	##%%%s  AVSV!AU0 =S>>U0 <S=U0 =T ?S T SS3SSST SBT S0)T (T -S3.T 1T S6A:T;S9<&T"S<
#AT0S?1)TB&TT TT #U0 4T5!U0 TU0 #V?U. VU0 U0 T SST S- S#!S-(	T 3T 6T9T<T?TT T
TTT U0 U0 $U+U"U+$U'%U++U0 .V0$VVVV)r   )builtinsr   _pytest.assertion.rewrite	assertionrewriter5   rJ   r   r   r   r   edge_proxy.serverr   r   rA   r   floatlistrO   markr   r   r   r   r(   r   r   <module>r      s           -> >B<
X 
X% 
XRVWZR[ 
X H& H&V =& =&@ 7& 7&t N& N&r   