def _cancel_goal_async(self, goal_handle):
"""
Send a cancel request for an active goal and asynchronously get the result.
:param goal_handle: Handle to the goal to cancel.
:type goal_handle: :class:`ClientGoalHandle`
:return: a Future instance that completes when the cancel request has been processed.
:rtype: :class:`rclpy.task.Future` instance
"""
if not isinstance(goal_handle, ClientGoalHandle):
raise TypeError(
'Expected type ClientGoalHandle but received {}'.format(type(goal_handle)))
cancel_request = CancelGoal.Request()
cancel_request.goal_info.goal_id = goal_handle.goal_id
sequence_number = _rclpy_action.rclpy_action_send_cancel_request(
self._client_handle,
cancel_request)
if sequence_number in self._pending_cancel_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending cancel request'.format(sequence_number))
future = Future()
self._pending_cancel_requests[sequence_number] = future
future.add_done_callback(self._remove_pending_cancel_request)
# Add future so executor is aware
self.add_future(future)
return future
def test_cancel_goal_accept(self):
def execute_callback(goal_handle):
# Wait, to give the opportunity to cancel
time.sleep(3.0)
self.assertTrue(goal_handle.is_cancel_requested)
goal_handle.canceled()
return Fibonacci.Result()
def cancel_callback(request):
return CancelResponse.ACCEPT
executor = MultiThreadedExecutor(context=self.context)
action_server = ActionServer(
self.node,
Fibonacci,
'fibonacci',
callback_group=ReentrantCallbackGroup(),
execute_callback=execute_callback,
handle_accepted_callback=lambda gh: None,
cancel_callback=cancel_callback,
)
goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
goal_order = 10
goal_msg = Fibonacci.Impl.SendGoalService.Request()
goal_msg.goal_id = goal_uuid
goal_msg.goal.order = goal_order
goal_future = self.mock_action_client.send_goal(goal_msg)
rclpy.spin_until_future_complete(self.node, goal_future, executor)
goal_handle = goal_future.result()
self.assertTrue(goal_handle.accepted)
cancel_srv = CancelGoal.Request()
cancel_srv.goal_info.goal_id = goal_uuid
cancel_srv.goal_info.stamp.sec = 0
cancel_srv.goal_info.stamp.nanosec = 0
cancel_future = self.mock_action_client.cancel_goal(cancel_srv)
rclpy.spin_until_future_complete(self.node, cancel_future, executor)
cancel_result = cancel_future.result()
self.assertEqual(len(cancel_result.goals_canceling), 1)
assert all(
cancel_result.goals_canceling[0].goal_id.uuid == goal_uuid.uuid)
action_server.destroy()
executor.shutdown()
def test_cancel_defered_goal(self):
server_goal_handle = None
def handle_accepted_callback(gh):
nonlocal server_goal_handle
server_goal_handle = gh
def cancel_callback(request):
return CancelResponse.ACCEPT
def execute_callback(gh):
# The goal should already be in state CANCELING
self.assertTrue(gh.is_cancel_requested)
gh.canceled()
return Fibonacci.Result()
action_server = ActionServer(
self.node,
Fibonacci,
'fibonacci',
callback_group=ReentrantCallbackGroup(),
execute_callback=execute_callback,
handle_accepted_callback=handle_accepted_callback,
cancel_callback=cancel_callback,
)
goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
goal_msg = Fibonacci.Impl.SendGoalService.Request()
goal_msg.goal_id = goal_uuid
goal_future = self.mock_action_client.send_goal(goal_msg)
rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
send_goal_response = goal_future.result()
self.assertTrue(send_goal_response.accepted)
self.assertIsNotNone(server_goal_handle)
self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_ACCEPTED)
# Cancel the goal, before execution
cancel_srv = CancelGoal.Request()
cancel_srv.goal_info.goal_id = goal_uuid
cancel_srv.goal_info.stamp.sec = 0
cancel_srv.goal_info.stamp.nanosec = 0
cancel_future = self.mock_action_client.cancel_goal(cancel_srv)
rclpy.spin_until_future_complete(self.node, cancel_future,
self.executor)
cancel_result = cancel_future.result()
self.assertEqual(len(cancel_result.goals_canceling), 1)
self.assertEqual(server_goal_handle.status,
GoalStatus.STATUS_CANCELING)
# Execute the goal
server_goal_handle.execute()
# Get the result and exepect it to have canceled status
get_result_future = self.mock_action_client.get_result(goal_uuid)
rclpy.spin_until_future_complete(self.node, get_result_future,
self.executor)
result = get_result_future.result()
self.assertEqual(result.status, GoalStatus.STATUS_CANCELED)
self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELED)
action_server.destroy()
