
    zin                         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
mZ d dlmZ ej                  j                  d        Zej                  j                  d        Zy)    N)Pose2D)RosbridgeBackendc            	      2	  K   g g fd} t        j                  | dd       d {   }	 |j                  d   j                         d   }t	        d| d      }|j                          d {    	 fd}|j                  d	d
t        ddd      d|       d {    |j                          d {    D cg c]  \  }}|	 }}}d}||v }	|	st        j                  d|	fd||f      t        j                  |      dt        j                         v st        j                  |      rt        j                  |      nddz  }
dd|
iz  }t        t        j                   |            d x}}	d}||v }	|	st        j                  d|	fd||f      t        j                  |      dt        j                         v st        j                  |      rt        j                  |      nddz  }
dd|
iz  }t        t        j                   |            d x}}	d}||v }	|	st        j                  d|	fd||f      t        j                  |      dt        j                         v st        j                  |      rt        j                  |      nddz  }
dd|
iz  }t        t        j                   |            d x}}	D cg c]  }|j#                  d      dk(  s| }}D cg c]  }|j#                  d      dk(  s| }}|s{t        j$                  d      dz   dd t        j                         v st        j                  |      rt        j                  |      nd iz  }t        t        j                   |            |s{t        j$                  d!      dz   dd"t        j                         v st        j                  |      rt        j                  |      nd"iz  }t        t        j                   |            	 |j'                          |j)                          d {    y 7 7 7 7 }# |j                          d {  7   w xY wc c}}w c c}w c c}w 7 ?# |j'                          |j)                          d {  7   w xY ww)#Nc           
        K   | 2 3 d {   }t        j                  |      }|j                  d      dk7  r3j                  |       |j                  d      dk7  rY| j	                  t        j
                  ddddid             d {    | j	                  t        j
                  dd	d
did
digddd             d {    | j	                  t        j
                  ddddid             d {    7 7 r7 ;7 6 y w)Noppublishtopicgui/execute_planzgui/get_plan_feedbackdata   r   r	   msgzgui/get_waypoints_newstatus   )	waypointsnext_waypointr   )jsonloadsgetappendsenddumps)wsrawr   	publisheds      J/home/nelsen/Projects/kognitive/edge-proxy/tests/test_rosbridge_backend.pyrosbridge_handlerz?test_rosbridge_backend_navigate_flow.<locals>.rosbridge_handler   s0     )	 )	#**S/Cwwt}	)S!www#55''

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

'!8 "*1!)1* ./ 
   ''

'!8 &{  C)	C s\   DDC=DA>DC?8DD1D7D8D=D?DDDD	127.0.0.1r   r   ws://127.0.0.1:Frosbridge_urlinsecure_tlsc                 \   K   j                  | j                  | j                  f       y w)N)r   r   progress)updateupdatess    r   
_on_updatez8test_rosbridge_backend_navigate_flow.<locals>._on_updateE   s      v?@s   ),nav_001officeg      ?g       @g      ?)xythetanormal)
request_iddestinationposespeed	on_updateaccepted)in)z%(py1)s in %(py3)sstatuses)py1py3zassert %(py5)spy5
navigatingarrivedr	   zgui/add_waypoint_newr
   z(No gui/add_waypoint_new publish observedz
>assert %(py0)spy0add_waypoint_msgsz$No gui/execute_plan publish observedexecute_msgs)
websocketsservesocketsgetsocknamer   startnavigate_to_poser   stop
@pytest_ar_call_reprcompare	_saferepr@py_builtinslocals_should_repr_global_nameAssertionError_format_explanationr   _format_assertmsgclosewait_closed)r   	ws_serverportbackendr'   s_r5   @py_assert0@py_assert2@py_format4@py_format6mr<   r=   @py_format1r   r&   s                   @@r   $test_rosbridge_backend_navigate_flowr[      s$    GI*X !&&'8+qIII &  #//1!4"+D62
 mmo	!A **$$cS4$ +    ,,.  ")*$!QA**%zX%%%%zX%%%z%%%%%%X%%%X%%%%%%%'|x''''|x'''|''''''x'''x'''''''$yH$$$$yH$$$y$$$$$$H$$$H$$$$$$$(1^1QUU7^G]5]Q^^#,UagBT0TUU LL"LLLLLLL LLL LLLLLCCCCCCCCC|CCC|CCCCC|##%%%C J 	
 !',,.  *
 _U
 	& 	##%%%s   $RP-RAQ) 1P02Q) 7*P9 !P3"P9 &Q) 9P6:	Q) QG6Q) QQ#Q) )Q"Q"C<Q) #R'Q'(R0Q) 3P9 6Q) 9QQQQ) 'R)$RRRRc            	        K   dfd} t        j                  | dd       d {   }	 |j                  d   j                         d   }t	        d| d      }|j                          d {    	 t        j                  d       d {    |j                         }|j                         }|j                  }t        j                  }d	} ||      }	||	k(  }
|
st        j                  d
|
fd||	f      dt        j                          v st        j"                  |      rt        j$                  |      ndt        j$                  |      dt        j                          v st        j"                  t              rt        j$                  t              ndt        j$                  |      t        j$                  |      t        j$                  |	      dz  }dd|iz  }t'        t        j(                  |            d x}x}
x}x}}	|j*                  }t        j                  }d}| }	 ||	      }||k(  }
|
st        j                  d
|
fd||f      dt        j                          v st        j"                  |      rt        j$                  |      ndt        j$                  |      dt        j                          v st        j"                  t              rt        j$                  t              ndt        j$                  |      t        j$                  |      t        j$                  |      dz  }dd|iz  }t'        t        j(                  |            d x}x}
x}x}x}	}|j,                  }t        j                  }d} ||      }	||	k(  }
|
st        j                  d
|
fd||	f      dt        j                          v st        j"                  |      rt        j$                  |      ndt        j$                  |      dt        j                          v st        j"                  t              rt        j$                  t              ndt        j$                  |      t        j$                  |      t        j$                  |	      dz  }dd|iz  }t'        t        j(                  |            d x}x}
x}x}}	|j.                  }d}||k(  }
|
st        j                  d
|
fd||f      dt        j                          v st        j"                  |      rt        j$                  |      ndt        j$                  |      t        j$                  |      dz  }dd|iz  }t'        t        j(                  |            d x}x}
}|j0                  }d}||u }
|
st        j                  d|
fd ||f      dt        j                          v st        j"                  |      rt        j$                  |      ndt        j$                  |      t        j$                  |      dz  }dd|iz  }t'        t        j(                  |            d x}x}
}|j3                          d {    	 |j5                          |j7                          d {    y 7 d7 7 7 7# |j3                          d {  7   w xY w7 .# |j5                          |j7                          d {  7   w xY ww)!NFc                 `  K   | 2 3 d {   }t        j                  |      }|j                  d      dk(  s3r6d| j                  t        j                  dddddd	d
             d {    | j                  t        j                  dddddd
             d {    7 7 :7 	6 y w)Nr   	subscribeTr   zgui/get_robot_pose皙	@g      ffffff?)r*   r+   zr   z/kstack/state/batterygףp=
?r   )
percentagepower_supply_status)r   r   r   r   r   )r   r   r   sent_telemetrys      r   r   zCtest_rosbridge_backend_telemetry_updates.<locals>.rosbridge_handlerd   s      	 	#**S/Cwwt}+N!%ggJJ"+%9),4c#B   ggJJ"+%<26q#Q  	 sS   B.B,B&B,*B.B.1B.-B(.2B. B*!B.&B,(B.*B.,B.r   r   r   r   r    g?r_   )==)ze%(py2)s
{%(py2)s = %(py0)s.x
} == %(py10)s
{%(py10)s = %(py6)s
{%(py6)s = %(py4)s.approx
}(%(py8)s)
}r0   pytest)r;   py2py4py6py8py10zassert %(py12)spy12g      ?)zf%(py2)s
{%(py2)s = %(py0)s.y
} == %(py11)s
{%(py11)s = %(py6)s
{%(py6)s = %(py4)s.approx
}(-%(py8)s)
})r;   rg   rh   ri   rj   py11zassert %(py13)spy13r`   )zi%(py2)s
{%(py2)s = %(py0)s.theta
} == %(py10)s
{%(py10)s = %(py6)s
{%(py6)s = %(py4)s.approx
}(%(py8)s)
}>   )z-%(py2)s
{%(py2)s = %(py0)s.level
} == %(py5)sbattery)r;   rg   r8   zassert %(py7)spy7T)is)z0%(py2)s
{%(py2)s = %(py0)s.charging
} is %(py5)s)r>   r?   r@   rA   r   rB   asynciosleepget_poseget_batteryr*   rf   approxrE   rF   rH   rI   rJ   rG   rK   rL   r+   r,   levelchargingrD   rN   rO   )r   rP   rQ   rR   r0   rp   @py_assert1@py_assert5@py_assert7@py_assert9@py_assert3@py_format11@py_format13@py_assert10@py_format12@py_format14@py_assert4rX   @py_format8rd   s                      @r   (test_rosbridge_backend_telemetry_updatesr   `   s/    N2 !&&'8+qIII&  #//1!4"+D62
 mmo
	!--$$$##%D))+G66/V]]/3/]3//6/////6///////4///4///6//////V///V///]///3///////////660V]]0C0C40]40060000060000000400040006000000V000V000]000C000000000000::33s3s!33:!33333:!333333343334333:333333333333333s333!33333333==&B&=B&&&&=B&&&&&&7&&&7&&&=&&&B&&&&&&&##+t+#t++++#t++++++7+++7+++#+++t+++++++,,.  ##%%%+ J 	$ !',,.   	& 	##%%%s   "["Z	["AZ4 .Z/Z4 4Z ZV:Z Z4 ZZ4  #["Z2["Z4 Z Z4 Z/(Z+)Z//Z4 2["4$[[[[")builtinsrH   _pytest.assertion.rewrite	assertionrewriterE   rs   r   rf   r>   edge_proxy.nav_backend.baser   (edge_proxy.nav_backend.rosbridge_backendr   markr[   r        r   <module>r      sZ          . E Q& Q&h 1& 1&r   