
    i8                        d dl 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 d dl	m
Z
mZmZ d dlmZ d dlZd dlmZ d dlmZ d dlmZmZ d	d
lmZmZmZmZ  ed       G d d             Z G d de      Zy)    )annotationsN)	dataclass)AnyDictOptional)urlparse)WebSocketClientProtocol)ConnectionClosed)BatteryPose   )
NavBackend	NavUpdateNavUpdateCallbackPose2DT)frozenc                  "    e Zd ZU ded<   ded<   y)_TopicConfigstrtopicmsg_typeN)__name__
__module____qualname____annotations__     6edge-proxy/edge_proxy/nav_backend/rosbridge_backend.pyr   r      s    JMr   r   c                     e Zd ZdZdZdZdZdZdZdZ	dZ
dZdZddd	 	 	 	 	 dd	Zd d
Zd 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d*dZd*dZd+dZd)dZddd,dZd-dZd.dZy)/RosbridgeBackendzHNav backend using rosbridge WebSocket and waypoint_handler gui/* topics.r   r      zwss://127.0.0.1:9090T)rosbridge_urlinsecure_tlsc                  || _         || _        d | _        d | _        t	        j
                         | _        t	        j
                         | _        t               | _	        d | _
        t               | _        d | _        d | _        d | _        d| _        d| _        d| _        d| _        t)        dd      t)        dd      t)        d	d      f| _        t)        d
d      t)        dd      t)        dd      t)        dd      f| _        y )Nidle        F      gui/add_waypoint_newz&waypoint_handler_idl/msg/WaypointArraygui/execute_planzstd_msgs/msg/Emptygui/cancel_plangui/get_plan_feedbackzstd_msgs/msg/Int8gui/get_waypoints_newgui/get_robot_posezgeometry_msgs/msg/Point/kstack/state/batteryzsensor_msgs/msg/BatteryState)_rosbridge_url_insecure_tls_ws_receiver_taskasyncioLock
_send_lock_state_lockr   _latest_pose_latest_pose_tsr   _latest_battery_active_request_id_active_on_update_active_done_active_phase_active_progress_active_cancelled_last_progress_sentr   _publisher_topics_subscriber_topics)selfr#   r$   s      r   __init__zRosbridgeBackend.__init__*   s     ,)6:<@!,,."<<> F04&y15>B59"('*',*.  /1YZ+-AB*,@A"
 02EF02Z[-/HI02PQ	#
r   c                  K   | j                         }t        j                  | j                  dd|       d {   | _        | j                          d {    t        j                  | j                               | _	        y 7 J7 /w)N   )ping_intervalping_timeoutssl)
_build_ssl_context
websocketsconnectr0   r2   _register_rosbridge_topicsr4   create_task_receiver_loopr3   )rD   ssl_ctxs     r   startzRosbridgeBackend.startP   sx     ))+#++	
 
 --///%11$2E2E2GH
 	0s!   7BBBB.BBc                `  K   | j                   6| j                   j                          	 | j                    d {    d | _         | j                  8| j                  j
                  s"| j                  j                          d {    d | _        y 7 W# t        j                  $ r Y jw xY w7 'wN)r3   cancelr4   CancelledErrorr2   closedcloserD   s    r   stopzRosbridgeBackend.stop[   s     *&&()))) #'D88((..""" *)) 
 #sD   'B.B BB AB.B,B.B B)&B.(B))B.c           	     P  K   |}|}|}| j                   4 d {    | j                  t        d      || _        || _        t	        j
                         | _        d| _        d| _        d| _	        d| _
        d d d       d {     |t        dd             d {    	 | j                  |       d {    t	        j                  | j                  j                         d	       d {    | j                   4 d {    d | _        d | _        d | _        d| _        d| _	        d d d       d {    y 7 $7 # 1 d {  7  sw Y   xY w7 7 7 k# t        j                  $ r6 | j!                  t        d
| j                  dd      d       d {  7   Y w xY w7 7 s# 1 d {  7  sw Y   y xY w# | j                   4 d {  7   d | _        d | _        d | _        d| _        d| _	        d d d       d {  7   w # 1 d {  7  sw Y   w xY wxY ww)Nz,Another navigation request is already activependingr'   Fr(   acceptedstatusprogressg      ^@)timeoutfailedra   NAV_TIMEOUTr_   r`   reason
error_codeTset_doner&   )r7   r;   RuntimeErrorr<   r4   Eventr=   r>   r?   r@   rA   r   $_publish_single_waypoint_and_executewait_forwaitTimeoutError_emit_update)rD   
request_iddestinationposespeed	on_updateframe_id_s           r   navigate_to_posez!RosbridgeBackend.navigate_to_poseh   s     ## 
	, 
	,&&2"#QRR&0D#%.D" 'D!*D$'D!%*D"'+D$
	, 
	, 	cBCCC	/;;DAAA""4#4#4#9#9#;UKKK '' / /*.')-&$(!%+").&/ / /9
	, 
	, 
	, 
	, 
	, 	D BK## 		###!22$,	  $   		/ / / / /t'' / /*.')-&$(!%+").&/ / / / /s6  H&D=H&AE8H&E H&EH&$E 8E96E /E0E 4H&F)H&$F-,H&7F+8H& H&EE	EH&E E A F&F F&#G %F&&G )H&+H&-F?3F64F?;H&H#G
H#$H<H#H
H#H HH H##H&c                X  K   |}| j                   4 d {    | j                  |k7  r	 d d d       d {    y d| _        | j                  }d d d       d {    | j	                  di        d {    |j                          y y 7 w7 Y7 7# 1 d {  7  sw Y   GxY w7 3w)NTr+   r   msg)r7   r;   r@   r=   _publish_messageset)rD   rp   re   rv   done_evts        r   rU   zRosbridgeBackend.cancel   s     ## 	) 	)&&*4	) 	) 	) &*D"((H		) 	) ### $ 
 	
 	

 LLN  	) 	) 	) 	) 	) 	)	
sj   B*BB*BB*BB*BB*BB*4B(5B*B*B*B%BB%!B*c                    | j                   S rT   )r8   rY   s    r   get_posezRosbridgeBackend.get_pose   s       r   c                r    | j                   yt        dt        j                         | j                   z
        S )zDReturn age of latest pose sample in seconds, or None if unavailable.Nr'   )r9   maxtime	monotonicrY   s    r   get_pose_age_secz!RosbridgeBackend.get_pose_age_sec   s0    '3(4+?+??@@r   c                    | j                   S rT   )r:   rY   s    r   get_batteryzRosbridgeBackend.get_battery   s    ###r   c                    t        | j                        j                  j                         }|dk7  ry t	        j
                         }| j                  rd|_        t        j                  |_	        |S )NwssF)
r   r0   schemelowerrJ   create_default_contextr1   check_hostname	CERT_NONEverify_mode)rD   r   ctxs      r   rK   z#RosbridgeBackend._build_ssl_context   sZ    $--.55;;=U?((*!&C!mmCO
r   c                  K   | j                   D ]3  }| j                  d|j                  |j                  d       d {    5 | j                  D ]3  }| j                  d|j                  |j                  d       d {    5 y 7 I7 	w)N	advertise)opr   type	subscribe)rB   	_send_rawr   r   rC   )rD   cfgs     r   rN   z+RosbridgeBackend._register_rosbridge_topics   s     )) 	C..% YYLL  	 ** 	C..% YYLL  	s"   <BBAB B
B
Bc           
     V  K   | j                   }|y 	 |2 3 d {   }t        |t        t        f      r	 |j	                  dd      }t        |t              sD	 t        j                  |      }t        |t              sk|j                  d      dk7  rt        |j                  dd            }|j                  d      }t        |t              s| j                  ||       d {    7 # t
        $ r Y w xY w# t        j                  $ r Y w xY w7 /6 n"# t        j                  $ r  t        $ r Y nw xY w| j!                  t#        d	| j$                  d
d      d       d {  7   y # | j!                  t#        d	| j$                  d
d      d       d {  7   w xY ww)Nzutf-8replace)errorsr   publishr    rz   rb   rosbridge_disconnectedNAV_LOCALIZATION_LOSTrd   Trg   )r2   
isinstancebytes	bytearraydecode	Exceptionr   jsonloadsJSONDecodeErrordictget_handle_publishr4   rV   r
   ro   r   r?   )rD   wsrawpayloadr   rz   s         r   rP   zRosbridgeBackend._receiver_loop   s    XX:)	 7 7ccE9#56!!jjjC "#s+"jjoG "'40;;t$	1GKK45kk%(!#t,**5#66657 % ! ! ++  75  6 %% 	 		 ###!2236	  $   $###!2236	  $   s   F)D DC-DD C/D C>/A8D 'D(D -D/	C;8D :C;;D >DD DD D E1 D:7E1 9D::E1 =-F)*E-+F)1.F&F" F&&F)c           
       K   |dk(  rxt        t        |j                  dd            t        |j                  dd            t        |j                  dd                  | _        t	        j
                         | _        y |dk(  rt        |j                  dd	            }|d	k  rt        t        |d
z              }nt        t        |            }t        |j                  dd            | j                  k(  }t        ||      | _        y |dk(  r| j                  |       d {    y |dk(  r| j                  |       d {    y y 7 %7 w)Nr.   xr'   yz)r   r   thetar/   
percentage      ?g      Y@power_supply_statusr   )levelchargingr-   r,   )r   floatr   r8   r   r   r9   intround_POWER_SUPPLY_STATUS_CHARGINGr   r:   _handle_waypoint_feedback_handle_plan_feedback)rD   r   rz   r   r   r   s         r   r   z RosbridgeBackend._handle_publish  s5    (( $S)*S)*CGGC-.!D
 $(>>#3D ++sww|S9:JS E*u"456E*-.377#8!<=AcAccH#*#JD ++00555++,,S111 , 6 2s$   D#E%E&EEEEc           
       K   |j                  d      }t        |t              r|sy d}d}|D ]]  }t        |t              s|dz  }t	        |j                  d| j
                              }|| j                  | j                  fv sY|dz  }_ |dk(  ry t        dt        d|t        |      z              }t        j                  || j                  d      ry || _        | j                  t        d	|
             d {    y 7 w)N	waypointsr   r   r_   r'   gGz?g{Gz?)abs_tol
navigatingr^   )r   r   listr   r   _WP_STATUS_NOT_REACHED_WP_STATUS_REACHED_WP_STATUS_FAILEDr   minr   mathiscloserA   ro   r   )rD   rz   r   reachedtotalwpr_   r`   s           r   r   z*RosbridgeBackend._handle_waypoint_feedback  s     GGK(	)T*) 	Bb$'QJE$*E*EFGF$1143I3IJJ1	 A:sCge&<=><<$":":DI#+ 	 QRRRs   BDA8D>D?Dc                   K   t        |j                  d| j                              }| j                  4 d {    | j                  	 d d d       d {    y | j
                  }| j                  }d d d       d {    || j                  k(  ri| j                  4 d {    | j
                  dv rd| _        d d d       d {    | j                  t        d| j                               d {    y || j                  k(  r/| j                  t        d| j                               d {    y || j                  k(  r_| j                  4 d {    d| _        d d d       d {    | j                  t        d| j                  dd	      d
       d {    y || j                  k(  rr]| j                  4 d {    d| _        d d d       d {    | j                  t        d| j                        d
       d {    y dk(  rZ| j                  4 d {    d| _        d| _
        d d d       d {    | j                  t        dd      d
       d {    y y y 7 >7 $7 # 1 d {  7  sw Y   xY w7 7 # 1 d {  7  sw Y   xY w7 7 7 _7 K# 1 d {  7  sw Y   \xY w7 27 7 # 1 d {  7  sw Y   
xY w7 7 7 # 1 d {  7  sw Y   xY w7 w)Ndata>   activer\   r   r   r^   rb   waypoint_handler_failedNAV_BLOCKEDrd   Trg   	cancelledcompleter   arrived)r   r   _PLAN_INACTIVEr7   r;   r>   r@   _PLAN_ACTIVEro   r   r?   _PLAN_PAUSED_PLAN_FAILED)rD   rz   r_   phaser   s        r   r   z&RosbridgeBackend._handle_plan_feedback7  s    SWWVT%8%89:## 	/ 	/&&.	/ 	/ 	/ &&E..I	/ 	/ T&&&'' 2 2%%)>>)1D&2 2 ##I\DLaLa$bcccT&&&##I\DLaLa$bcccT&&&'' . .%-". .###!224,	  $    T(((++ 5 5)4D&5 5''*!%!6!6 " (     ++ 0 0)3D&,/D)0 0 ''(!$ " (   	 ! )A	/ 	/ 	/ 	/ 	/ 	/2 2 2 2 2 d d. . . . .5 5 5 5 50 0 0 0 0s  6LI6LI?
LI9LI?3L>I<?#L"J#L&J<LJ-L5J16=L3J44$LJ7LJ=$L/J:01L!K"&LK	LKLK /LK1L*K3+L.K7=LK5	%L.L/L9L<L?JJJ	LLJ.!J$"J.)	L4L7L:L=KKK	LLLK.!K$"K.)	L3L5L7L	=L >L	Lc                  K   t        j                  t        |j                        dz        }t        j                  t        |j                        dz        }| j                         \  }}||dddt        |j                        t        |j                        dddd||ddd	d	d
gd	d}| j                  d|       d {    | j                  di        d {    y 7  7 w)Ng      ?)secnanosecmap)stampru   r'   )r   r   r   )r   r   r   w)positionorientationr   )rr   moder_   )headerr   next_waypointr)   ry   r*   )	r   sinr   r   cos
_stamp_nowr   r   r{   )rD   rr   qzqwr   r   waypoint_array_msgs          r   rk   z5RosbridgeBackend._publish_single_waypoint_and_executes  s    XXeDJJ'#-.XXeDJJ'#-.(W "%9! "'tvv!&tvv!$% "%!$!#!#	( & 1
6 ##(" $ 
 	
 	
 ##$ $ 
 	
 	
		
	
s$   CC*C&C* C(!C*(C*c               J   K   | j                  d||d       d {    y 7 w)Nr   )r   r   rz   )r   )rD   r   rz   s      r   r{   z!RosbridgeBackend._publish_message  s*     nn
 	
 	
s   #!#Frg   c               X  K   | j                   4 d {    | j                  }| j                  }|j                  t	        |j                        | _        d d d       d {     ||       d {    |r|j                          y y y 7 {7 /# 1 d {  7  sw Y   ?xY w7 4wrT   )r7   r<   r=   r`   r   r?   r|   )rD   updaterh   callbackr}   s        r   ro   zRosbridgeBackend._emit_update  s     ## 	? 	?--H((H*(-foo(>%		? 	? 6""",LLN -8	? 	? 	? 	? 	? #sP   B*BB*?BB*!B"B*3B(4B*B*B%BB%!B*c                .  K   | j                   }||j                  rt        d      | j                  4 d {    |j	                  t        j                  |             d {    d d d       d {    y 7 A7 7 	# 1 d {  7  sw Y   y xY ww)Nzrosbridge is not connected)r2   rW   ri   r6   sendr   dumps)rD   r   r   s      r   r   zRosbridgeBackend._send_raw  s{     XX:;<<?? 	/ 	/''$**W-...	/ 	/ 	/.	/ 	/ 	/ 	/sV   6BA:B(B $A<%B )B4A>5B<B >B BB	BBc                j    t        j                          }t        |      }t        ||z
  dz        }||fS )Ni ʚ;)r   r   )rD   tr   r   s       r   r   zRosbridgeBackend._stamp_now  s3    IIK!fq3w-/0G|r   N)r#   r   r$   boolreturnNone)r   r   )r   )rp   r   rq   r   rr   r   rs   r   rt   r   ru   r   r   r   )r   )rp   r   re   r   r   r   )r   r   )r   zOptional[float])r   r   )r   zOptional[ssl.SSLContext])r   r   rz   Dict[str, Any]r   r   )rz   r   r   r   )rr   r   r   r   )r   r   rh   r   r   r   )r   r   r   r   )r   ztuple[int, int]) r   r   r   __doc__r   r   r   r   r   r   _WP_STATUS_CURRENTr   r   rE   rR   rZ   rw   rU   r   r   r   rK   rN   rP   r   r   r   rk   r{   ro   r   r   r   r   r   r    r       s   RLNLL$%!
 4!	$
 $
 	$

 
$
L	I( ././ ./ 	./
 ./ %./ ./ 
./` !A$	&.`8S2:x'
R
 IN /r   r    )
__future__r   r4   r   r   rJ   r   dataclassesr   typingr   r   r   urllib.parser   rL   websockets.clientr	   websockets.exceptionsr
   edge_proxy.messagesr   r   baser   r   r   r   r   r    r   r   r   <module>r      sa    "    
  ! & & !  5 2 - B B $  
cz cr   