Esempio n. 1
0
    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
Esempio n. 2
0
    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()
Esempio n. 3
0
    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()