
    &Si!                        d dl m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
mZ 	 d dlmZmZ  G d de      Zy# e$ r Y w xY w)	    )annotationsN)Optional   )
NavBackend	NavUpdateNavUpdateCallbackPose2DPoseBatteryc                  r    e Zd ZdZdddZddZddZddZddZddZ		 d	 	 	 	 	 	 	 	 	 	 	 	 	 ddZ
ddd	Zy
)Nav2BackendzNav2 backend using rclpy ActionClient.

    Runs rclpy spinning in a background thread; asyncio awaits by polling rclpy futures.
    c                &   || _         d | _        t        j                         | _        d | _        d | _        d | _        d | _        d | _	        d | _
        d | _        d | _        t        j                         | _        t        j                         | _        y )N)_action_name_spin_thread	threadingEvent_stop_event_rclpy_node	_executor_client_goal_handle_active_request_id_latest_pose_latest_batteryLock
_pose_lock_battery_lock)selfaction_names     Q/home/nelsen/Projects/kognitive/edge-proxy/edge_proxy/nav_backend/nav2_backend.py__init__zNav2Backend.__init__   s|    '8<$??,
 15 /348#..*&^^-    c                   K   	 dd l }ddlm} ddlm} ddlm} ddlm} ddl	m
} ddlm} dd	lmm | _        |j'                         s|j)                  d 
        |d       _         |        _         j,                  j/                   j*                          | j*                  | j0                         _        d fd}	 j*                  j5                  |d|	d       dd fd}
 j*                  j5                  |d|
d        j6                  j9                          t;        j<                   j>                  d       _          j@                  jC                          tE        d      D ]=  } j2                  jG                  d      r y tI        jJ                  d       d {    ? y # t        $ r}t        d|       d }~ww xY w7 &w)Nr   )SingleThreadedExecutor)Node)ActionClientNavigateToPose)Point)BatteryStatez>Nav2 backend requires ROS2 Python packages (rclpy/nav2_msgs): r
   )args
edge_proxyc                    j                   5   | j                  | j                  | j                        _        d d d        y # 1 sw Y   y xY w)N)xytheta)r   r0   r1   zr   )msgPoseMsgr    s    r"   _pose_cbz#Nav2Backend.start.<locals>._pose_cbE   s@     K$+ceesuuCEE$J!K K Ks   /AAzgui/get_robot_pose
   r   c                    t        t        t        | j                        dz              }t        | j                        k(  }j
                  5   ||      _        d d d        y # 1 sw Y   y xY w)Ng      Y@)levelcharging)introundfloat
percentagepower_supply_statusr   r   )r4   r9   r:   
BatteryMsg_POWER_SUPPLY_STATUS_CHARGINGr    s      r"   _battery_cbz&Nav2Backend.start.<locals>._battery_cbQ   sg    eCNN3e;<=E32237TTH## R'1'Q$R R Rs   A))A2z/kstack/state/batteryT)targetdaemon2   皙?timeout_sec皙?)r4   r+   returnNone)r4   r,   rJ   rK   )&rclpyrclpy.executorsr&   
rclpy.noder'   rclpy.actionr(   nav2_msgs.actionr*   geometry_msgs.msgr+   sensor_msgs.msgr,   	ExceptionRuntimeErroredge_proxy.messagesr   r   r   okinitr   r   add_noder   r   create_subscriptionr   clearr   Thread_spinr   startrangewait_for_serverasynciosleep)r    rL   r&   r'   r(   r*   r+   r,   excr6   rB   _r@   r5   rA   s   `           @@@r"   r]   zNav2Backend.start)   s    		g>'17/4
 	OxxzJJDJ!,'
/1

+#DJJ@Q@QR
	K 	

&&u.BHbQ )*%	R 	

&&|5Lk[]^ %,,DJJtL! r 	&A||+++<--%%%	&[  	g!_`c_deff	g` &s4   G(F5 E=G-G.G5	G>GGGc                    ddl m} | j                  5  | j                  | j                  cddd       S 	 ddd        |       S # 1 sw Y    |       S xY w)zKReturn the latest robot pose from ROS2, or a zero pose if not yet received.r   )r   N)rU   r   r   r   )r    r5   s     r"   get_posezNav2Backend.get_posed   sR    7__ 	)  ,((	) 	),	) y	) y   AAc                    ddl m} | j                  5  | j                  | j                  cddd       S 	 ddd        |       S # 1 sw Y    |       S xY w)zTReturn the latest battery state from ROS2, or 100% not-charging if not yet received.r   )r   N)rU   r   r   r   )r    r@   s     r"   get_batteryzNav2Backend.get_batterym   sT    = 	,##/++	, 	,/	, |	, |rf   c                    | j                   J | j                  j                         s8| j                   j                  d       | j                  j                         s7y y )NrF   rG   )r   r   is_set	spin_oncer    s    r"   r\   zNav2Backend._spinv   sM    ~~)))""))+NN$$$5 ""))+r$   c                :  K   | j                   j                         ry | j                   j                          | j                  | j                  j	                  d       | j
                  2| j                  &	 | j
                  j                  | j                         | j                  	 | j                  j                          | j                  	 | j                  j                          y y # t        $ r Y [w xY w# t        $ r Y Cw xY w# t        $ r Y y w xY ww)Ng       @)timeout)r   rj   setr   joinr   r   remove_noderS   destroy_noder   shutdownrl   s    r"   stopzNav2Backend.stop{   s     ""$(""3"/>>%$***@**4::6 ::!

'') ;;"$$& #      sr   A6D9%C. D+C= DD ,D.	C:7D9C::D=	D	DD		D	DDDDc                   K    j                   t        d      ddlm} ddlm}  _         t        dd             d {    t        j                         |j                         }	 |       }
||
j                  _         j                  j                         j                         j!                         |
j                  _        t%        |j&                        |
j(                  j*                  _        t%        |j,                        |
j(                  j*                  _        d|
j(                  j*                  _        t1        j2                  t%        |j4                        dz        }t1        j6                  t%        |j4                        dz        }||
j(                  j8                  _        ||
j(                  j8                  _        |
|	_        d fd	} j                   j=                  |	|
      }|j?                         s.t        j@                  d       d {    |j?                         s.|jC                         }| _"        |jF                  s t        ddd             d {    y |jI                         }|j?                         s.t        j@                  d       d {    |j?                         s.|jC                         }tK        |dd       }|dk(  r t        dd             d {    y |dk(  r t        d             d {    y  t        dd             d {    y 7 7 !7 7 7 K7 -7 w)NzNav2 backend not startedr   r)   )PoseStampedacceptedg        )statusprogressg      ?c                    j                   k7  ry 	 t        j                   t        d                   y # t        $ r Y y w xY w)N
navigatingrx   )r   r`   run_coroutine_threadsafer   rS   )feedback_msgloop	on_update
request_idr    s    r"   feedback_cbz1Nav2Backend.navigate_to_pose.<locals>.feedback_cb   sM     &&*400i|<=  s   &: 	AA)feedback_callbackrI   failedgoal_rejectedNAV_INVALID_GOAL)rx   reason
error_coderF   rx      arrivedg      ?   	cancelledr|   nav2_failed)rx   r   rJ   rK   )&r   rT   rP   r*   rQ   rv   r   r   r`   get_running_loopGoalheaderframe_idr   	get_clocknowto_msgstampr=   r0   posepositionr1   r3   mathsinr2   cosorientationwsend_goal_asyncdonera   resultr   rw   get_result_asyncgetattr)r    r   destinationr   speedr   r   r*   rv   goal_msgstampedqzqwr   send_goal_futuregoal_handleresult_futurer   status_coder   s   ``   `             @r"   navigate_to_posezNav2Backend.navigate_to_pose   s     <<9::31",	cBCCC'') "&&(-"*#zz33599;BBD"'-"'-"%XXeDJJ'#-.XXeDJJ'#-.%'  "%'  "	 	   <<77T_7` #'')--%%% #'')&--/'##/N`a    $446$$&--$$$  $$& %%'fh5 !IYEFFF !I[9:::	-HIIII 	DP &
 % G
 ; 	Js   AMMGM%M&M;:M5M	6=M3M4M	8MMM!M"M=M>MM	MMMMMc                   K   | j                   y | j                  |k7  ry | j                   j                         }|j                         s/t	        j
                  d       d {    |j                         s.y y 7 w)NrI   )r   r   cancel_goal_asyncr   r`   ra   )r    r   r   cancel_futures       r"   cancelzNav2Backend.cancel   si     $""j0));;=$$&--%%%  $$&%s   A A<"A:#A<8A<N)z/navigate_to_pose)r!   strrJ   rK   r   )rJ   z'Pose')rJ   z	'Battery')map)r   r   r   r   r   r	   r   r   r   r   r   r   rJ   rK   ) )r   r   r   r   rJ   rK   )__name__
__module____qualname____doc__r#   r]   re   rh   r\   rt   r   r    r$   r"   r   r      s    
.$9&v6
B UJUJ UJ 	UJ
 UJ %UJ UJ 
UJn&r$   r   )
__future__r   r`   r   r   typingr   baser   r   r   r	   rU   r   r   ImportErrorr   r   r$   r"   <module>r      sD    "     B B	1
c&* c&	  		s   : AA