77from builtin_interfaces .msg import Duration
88from geometry_msgs .msg import Point
99from action_msgs .msg import GoalStatus
10+ from rclpy .action .client import ClientGoalHandle
1011
1112
1213class AutoDigNavOffloadServer (AsyncNode ):
@@ -31,6 +32,9 @@ def __init__(self):
3132 self .backup_in_progress = False
3233 self .offload_in_progress = False
3334
35+ self .dig_handle = ClientGoalHandle (None , None , None )
36+ self .offload_handle = ClientGoalHandle (None , None , None )
37+
3438 self .get_logger ().info ("Auto Dig-Backup-Offload server ready" )
3539
3640 async def execute_callback (self , goal_handle : ServerGoalHandle ):
@@ -54,109 +58,117 @@ async def execute_callback(self, goal_handle: ServerGoalHandle):
5458 goal_handle .abort ()
5559 return result
5660
57- if goal_handle .is_cancel_requested :
58- goal_handle .canceled ()
59- else :
61+ if not goal_handle .is_cancel_requested :
62+ self .get_logger ().info ("Auto Dig Nav Offload Procedure Complete!" )
6063 goal_handle .succeed ()
61- return result
64+ return result
65+ else :
66+ self .get_logger ().info ("Goal was cancelled" )
67+ goal_handle .canceled ()
68+ return result
6269
6370 async def _do_auto_dig (self , goal_handle ):
6471 self .get_logger ().info ("→ Starting AutoDig" )
6572 if not self ._auto_dig_client .wait_for_server (timeout_sec = 5.0 ):
6673 self .get_logger ().error ("AutoDig server unavailable" )
6774 return False
6875
69- self .dig_in_progress = True
70- dig_goal = AutoDig .Goal (
71- lift_digging_start_position = goal_handle .request .lift_digging_start_position ,
72- digger_chain_power = goal_handle .request .digger_chain_power ,
73- )
74- send = await self ._auto_dig_client .send_goal_async (dig_goal )
75- if not send .accepted :
76- self .get_logger ().error ("AutoDig rejected" )
77- self .dig_in_progress = False
78- return False
76+ if not goal_handle .is_cancel_requested :
77+ self .dig_in_progress = True
78+ dig_goal = AutoDig .Goal (
79+ lift_digging_start_position = goal_handle .request .lift_digging_start_position ,
80+ digger_chain_power = goal_handle .request .digger_chain_power ,
81+ )
82+ self .dig_handle = await self ._auto_dig_client .send_goal_async (dig_goal )
83+ if not self .dig_handle .accepted :
84+ self .get_logger ().error ("AutoDig rejected" )
85+ self .dig_in_progress = False
86+ return False
7987
80- await send .get_result_async ()
81- self .dig_in_progress = False
82- self .get_logger ().info ("→ AutoDig complete" )
83- return True
88+ await self . dig_handle .get_result_async ()
89+ self .dig_in_progress = False
90+ self .get_logger ().info ("→ AutoDig complete" )
91+ return True
8492
8593 async def _do_backup (self , goal_handle ):
86- dist = goal_handle .request .backward_distance
87- speed = 0.5 # duty cycle
88- timeout = 60.0 # seconds
89- self .get_logger ().info (f"→ Backing up { dist } { speed } )
90-
91- if not self ._backup_client .wait_for_server (timeout_sec = 5.0 ):
92- self .get_logger ().error ("BackUp server unavailable" )
93- return False
94-
95- target_point = Point ()
96- target_point .x = - abs (dist ) # negative x = backward
97- target_point .y = 0.0
98- target_point .z = 0.0
94+ if not goal_handle .is_cancel_requested :
95+ dist = goal_handle .request .backward_distance
96+ speed = 0.5 # duty cycle
97+ timeout = 60.0 # seconds
98+ self .get_logger ().info (f"→ Backing up { dist } { speed } )
9999
100- self .backup_in_progress = True
101- backup_goal = BackUp .Goal (
102- speed = speed ,
103- target = target_point ,
104- time_allowance = Duration (sec = int (timeout )),
105- )
106- send = await self ._backup_client .send_goal_async (backup_goal )
107- if not send .accepted :
108- self .get_logger ().error ("BackUp rejected" )
109- self .backup_in_progress = False
110- return False
100+ if not self ._backup_client .wait_for_server (timeout_sec = 5.0 ):
101+ self .get_logger ().error ("BackUp server unavailable" )
102+ return False
111103
112- # Wait for completion or cancel
113- result_future = send .get_result_async ()
114- while not result_future .done ():
115- if goal_handle .is_cancel_requested :
116- self ._backup_client .cancel_goal_async (send ) # ask Nav2 to stop
117- self .get_logger ().info ("BackUp canceled" )
104+ target_point = Point ()
105+ target_point .x = - abs (dist ) # negative x = backward
106+ target_point .y = 0.0
107+ target_point .z = 0.0
108+
109+ self .backup_in_progress = True
110+ backup_goal = BackUp .Goal (
111+ speed = speed ,
112+ target = target_point ,
113+ time_allowance = Duration (sec = int (timeout )),
114+ )
115+ send = await self ._backup_client .send_goal_async (backup_goal )
116+ if not send .accepted :
117+ self .get_logger ().error ("BackUp rejected" )
118118 self .backup_in_progress = False
119119 return False
120- await self .async_sleep (0.1 )
121120
122- result = await result_future
123- self .backup_in_progress = False
124- if result .status == GoalStatus .STATUS_SUCCEEDED :
125- self .get_logger ().info ("→ BackUp succeeded" )
126- return True
127- else :
128- self .get_logger ().error (f"BackUp failed: { result } )
129- return False
121+ # Wait for completion or cancel
122+ result_future = send .get_result_async ()
123+ while not result_future .done ():
124+ if goal_handle .is_cancel_requested :
125+ self ._backup_client .cancel_goal_async (send ) # ask Nav2 to stop
126+ self .get_logger ().info ("BackUp canceled" )
127+ self .backup_in_progress = False
128+ return False
129+ await self .async_sleep (0.1 )
130+
131+ result = await result_future
132+ self .backup_in_progress = False
133+ if result .status == GoalStatus .STATUS_SUCCEEDED :
134+ self .get_logger ().info ("→ BackUp succeeded" )
135+ return True
136+ else :
137+ self .get_logger ().error (f"BackUp failed: { result } )
138+ return False
130139
131140 async def _do_auto_offload (self , goal_handle ):
132141 self .get_logger ().info ("→ Starting AutoOffload" )
133142 if not self ._auto_offload_client .wait_for_server (timeout_sec = 5.0 ):
134143 self .get_logger ().error ("AutoOffload server unavailable" )
135144 return False
136145
137- self .offload_in_progress = True
138- offload_goal = AutoOffload .Goal ()
139- send = await self ._auto_offload_client .send_goal_async (offload_goal )
140- if not send .accepted :
141- self .get_logger ().error ("AutoOffload rejected" )
142- self .offload_in_progress = False
143- return False
146+ if not goal_handle .is_cancel_requested :
147+ self .offload_in_progress = True
148+ offload_goal = AutoOffload .Goal ()
149+ send = await self ._auto_offload_client .send_goal_async (offload_goal )
150+ if not send .accepted :
151+ self .get_logger ().error ("AutoOffload rejected" )
152+ self .offload_in_progress = False
153+ return False
144154
145- await send .get_result_async ()
146- self .offload_in_progress = False
147- self .get_logger ().info ("→ AutoOffload complete" )
148- return True
155+ await send .get_result_async ()
156+ self .offload_in_progress = False
157+ self .get_logger ().info ("→ AutoOffload complete" )
158+ return True
149159
150160 def cancel_callback (self , cancel_request ):
151161 super ().cancel_callback (cancel_request )
152162 self .get_logger ().info ("→ Cancellation requested" )
153163 if self .dig_in_progress :
154164 self .get_logger ().info ("Cancelling AutoDig" )
165+ self .dig_handle .cancel_goal_async () # cancel AutoDig
155166 if self .backup_in_progress :
156- self ._backup_client .cancel_all_goals () # cancel Nav2 backup
157167 self .get_logger ().info ("Cancelling BackUp" )
168+ self ._backup_client .cancel_all_goals () # cancel Nav2 backup
158169 if self .offload_in_progress :
159170 self .get_logger ().info ("Cancelling AutoOffload" )
171+ self .offload_handle .cancel_all_goals () # cancel AutoOffload
160172 return CancelResponse .ACCEPT
161173
162174
0 commit comments