
    Gi3                     N   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     5edge-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 sJ |d   dv sJ y 7 F7 w)Ntype)waypoint_listrobot_state)r   r   recv)r!   msg1msg2s      r   _drain_initialr0   L   sa     ::BGGIo&D::BGGIo&D<;;;;<;;;; &%s!   #A0A,*A0A.A0.A0
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_statusr1   r   >   failedarrived	cancelledz.Timed out waiting for terminal nav status for )	asyncioget_event_looptimer   r   r-   r   r    AssertionError)r!   r1   r2   statusesdeadliner   s         r   _collect_nav_statusesr?   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 ]Z  \  }	}
|j#                  t%        j&                  d|	d|
ddd             d {    t)        ||	       d {   }d|v sJ d|v rZJ  d d d       d {    |j*                  D cg c]  }|j-                  d      dk(  s| }}t/        |      dk(  sJ 	 |j1                          |j3                          d {    |j                  j5                          d {    	 |j1                          |j3                          d {    y 7 7 7 7 C7 37 7 7 # 1 d {  7  sw Y   xY wc c}w 7 7 _# |j1                          |j3                          d {  7   |j                  j5                          d {  7   w xY w7 # |j1                          |j3                          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*   r1   goalspeedr1   acceptedr7   r   r      )
write_textlstripr   
websocketsserver#   socketsgetsocknamer   str_backendstart_handlerrN   _process_requestconnectr0   r   r   r   r?   r	   r   lenclosewait_closedstop)tmp_pathr   fake_rb	rb_serverrb_portserveredge_ws_server	edge_portr!   r1   ra   r=   mexecute_msgss                 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$000#11 1* (/'8'8a!AEE'NN`<`AaLa|$)))  " ,,...//&&(((##%%%g H 	&
1(	  V#1 1 1 1* b /(   " ,,...//&&((( 	& 	##%%%sW  AL-I9L-!AK? =I<>>K? <I?=K? =J, ?J J, JJ;JJJ#J
$J3J6J, JJ, J#.J#2J, #K? (J()!K? 
J*K? #L-3K=4L-<K? ?K? J, JJ
JJ, J JJ J, (K? *K? ,$K:K"K:3K64K::K? =L-?$L*#L&$L**L-c                 B  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 sJ d d d       d {    |j0                  D 
cg c]  }
|
j3                  d      dk(  s|
 }}
|sJ d       |d    d!   }|d"   d#   d$k(  sJ |d%   d	   d&   d'   }|d(   t5        j6                  dd)*      k(  sJ |d+   t5        j6                  d,d)*      k(  sJ 	 |j9                          |j;                          d {    |j                  j=                          d {    	 |j9                          |j;                          d {    y 7 7 '7 7 7 7 7 O7 =7 *# 1 d {  7  sw Y   ;xY wc c}
w 7 7 f# |j9                          |j;                          d {  7   |j                  j=                          d {  7   w xY w7 # |j9                          |j;                          d {  7   w xY ww)-NrA   waypoints: []
g      $@      @r   rC   rG   rH   r   r   rI   rJ   rK   rL   FrM   rV   皙?r^   relative_forwardrelativeforward       @r*   	directiondistanceslowrc   rf   r7   r   zgui/add_waypoint_newz-Expected gui/add_waypoint_new to be publishedr   headerframe_idmapr   r   positionrD   gMbP?)absrE   g      @)ri   r   mathpirk   rl   r#   rm   rn   r   ro   rp   rq   rr   rN   rs   rt   r0   r9   sleepr   r   r   r?   r	   r   pytestapproxrv   rw   rx   )ry   r   rz   r{   r|   r}   r~   r   r!   r=   r   add_msgswaypoint_msgr   s                 r   6test_rosbridge_relative_goal_resolves_to_absolute_poser      sl    :%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 H,,,%- -( $+#4#4aagJ`8`aHaLLL8#B<.L)*5>>>,Q/7
CD9d ====9ct <<<<  " ,,...//&&(((##%%%q H 	&
-() Z#- - - -( b /(   " ,,...//&&((( 	& 	##%%%sd  ANK$N$AM1  K'>M1 ?K* M1 =L K-L K?K0K?2K334K?'K6(K?<K9=
K?L K<L %L?LA,L 0#M1 L!M1 5L6M1 ;#NM/N'M1 *M1 -L 0K?3K?6K?9K?<L ?LLLL M1 M1 $M,M"M,%M(&M,,M1 /N1$NNNNc                   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 sJ d d d       d {    |j(                  D 
cg c]  }
|
j+                  d      dk(  s|
 }}
|rJ d       	 |j-                          |j/                          d {    |j                  j1                          d {    	 |j-                          |j/                          d {    y 7 7 7 m7 -7 7 7 7 # 1 d {  7  sw Y   xY wc c}
w 7 7 _# |j-                          |j/                          d {  7   |j                  j1                          d {  7   w xY w7 # |j-                          |j/                          d {  7   w xY ww)NrA   r   rG   rH   r   r   rI   rJ   rK   rL   FrM   rV   r^   relative_failr   leftg      ?r   r   rc   rf   r6   r   r   z9No execute_plan publish expected when pose is unavailable)ri   r   rk   rl   r#   rm   rn   r   ro   rp   rq   rr   rN   rs   rt   r0   r   r   r   r?   r	   r   rv   rw   rx   )ry   r   rz   r{   r|   r}   r~   r   r!   r=   r   r   s               r   8test_rosbridge_relative_goal_fails_when_pose_unavailabler      s    :%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8+++%, ,( (/'8'8a!AEE'NN`<`AaLa#`%``#|  " ,,...//&&(((##%%%e H 	&
,( W#, , , ,( b /(   " ,,...//&&((( 	& 	##%%%sQ  AL	I
LAK +I,>K *I+K 0=J -I.J 1I& I4I&5I 6I&
I"
I&J  I$!J 3I;I;J #K  J !K "J#K (#LKLK K J I& I&"I&$J &I8,I/-I84J  K K $K(J+)"KKKK L$L;K><LL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(  sJ d d d       d {    |j=                          |j?                          d {    |j                  jA                          d {    	 |j=                          |j?                          d {    y 7 7 l7 07 7 7 7 # 1 d {  7  sw Y   xY w7 77 7 7 _7 ;7 7 # 1 d {  7  sw Y   xY w7 7 # |j=                          |j?                          d {  7   |j                  jA                          d {  7   w xY w7 # |j=                          |j?                          d {  7   w xY ww)%NrA   zH
waypoints:
  - name: office_scene
    x: 1.0
    y: 1.0
    theta: 0.0
rB   rC   rG   rH   r   r   rI   rJ   rK   rL   FrM   rV   r^   disconnect_caser_   rY   r`   rb   rc   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*   r1   r*   r,   	nav_stater7   g?)!ri   rj   r   rk   rl   r#   rm   rn   r   ro   rp   rq   rr   rN   rs   rt   r0   r   r   r   r9   r:   r;   anyr	   r   r<   r   r-   r   rv   rw   rx   )ry   r   rz   r{   r|   r}   r~   r   r!   execute_deadlinews2r   r>   r   s                 r   7test_navigation_continues_after_orchestrator_disconnectr   (  sZ    :%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@ !I---. .   " ,,...//&&(((##%%%C H 	&
(	   $ *.) c%5 -. . . .  /(   " ,,...//&&((( 	& 	##%%%s  AS>P&S>!AS =P)>>S <P,=S =Q= ?P/ Q= P;P23P;P5P;Q= P8BQ= Q)Q= (Q= -Q.Q= 1Q$ QA:Q$;Q<&Q$"Q
#AQ$0Q 1)Q$Q$"Q= -Q".Q= 2#S Q9!S 7Q;8S =#S> S!S>)S ,S /Q= 2P;5P;8Q= ;QQQ		Q= Q= Q$Q$Q$ Q$"Q= $Q6*Q-+Q62Q= 9S ;S =$S!R$""SSSS S>$S;4S75S;;S>)r   )r9   r   r   r   rk   edge_proxy.serverr   r   r0   ro   floatlistr?   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   