async def _execute_goal_request(self, request_header_and_message): request_header, goal_request = request_header_and_message goal_uuid = goal_request.goal_id goal_info = GoalInfo() goal_info.goal_id = goal_uuid self._node.get_logger().debug('New goal request with ID: {0}'.format( goal_uuid.uuid)) # Check if goal ID is already being tracked by this action server with self._lock: goal_id_exists = self._handle.goal_exists(goal_info) accepted = False if not goal_id_exists: # Call user goal callback response = await await_or_execute(self._goal_callback, goal_request.goal) if not isinstance(response, GoalResponse): self._node.get_logger().warning( 'Goal request callback did not return a GoalResponse type. Rejecting goal.' ) else: accepted = GoalResponse.ACCEPT == response if accepted: # Stamp time of acceptance goal_info.stamp = self._node.get_clock().now().to_msg() # Create a goal handle try: with self._lock: goal_handle = ServerGoalHandle(self, goal_info, goal_request.goal) except RuntimeError as e: self._node.get_logger().error( 'Failed to accept new goal with ID {0}: {1}'.format( goal_uuid.uuid, e)) accepted = False else: self._goal_handles[bytes(goal_uuid.uuid)] = goal_handle # Send response response_msg = self._action_type.Impl.SendGoalService.Response() response_msg.accepted = accepted response_msg.stamp = goal_info.stamp self._handle.send_goal_response(request_header, response_msg) if not accepted: self._node.get_logger().debug('New goal rejected: {0}'.format( goal_uuid.uuid)) return self._node.get_logger().debug('New goal accepted: {0}'.format( goal_uuid.uuid)) # Provide the user a reference to the goal handle await await_or_execute(self._handle_accepted_callback, goal_handle)
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from action_msgs.msg import GoalInfo self.goal_info = kwargs.get('goal_info', GoalInfo()) self.status = kwargs.get('status', int())