diff --git a/rclpy/rclpy/action/__init__.py b/rclpy/rclpy/action/__init__.py index 5e2b05413..8b5bf4ce5 100644 --- a/rclpy/rclpy/action/__init__.py +++ b/rclpy/rclpy/action/__init__.py @@ -13,3 +13,4 @@ # limitations under the License. from .client import ActionClient # noqa +from .server import ActionServer, CancelResponse, GoalResponse # noqa diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py new file mode 100644 index 000000000..d46dfabf3 --- /dev/null +++ b/rclpy/rclpy/action/server.py @@ -0,0 +1,595 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from enum import Enum +import functools +import threading + +from action_msgs.msg import GoalInfo, GoalStatus + +from rclpy.executors import await_or_execute +from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action +from rclpy.qos import qos_profile_action_status_default +from rclpy.qos import qos_profile_default, qos_profile_services_default +from rclpy.task import Future +from rclpy.type_support import check_for_type_support +from rclpy.waitable import NumberOfEntities, Waitable + + +class GoalResponse(Enum): + """Possible goal responses.""" + + REJECT = 1 + ACCEPT = 2 + + +class CancelResponse(Enum): + """Possible cancel responses.""" + + REJECT = 1 + ACCEPT = 2 + + +class GoalEvent(Enum): + """Goal events that cause state transitions.""" + + EXECUTE = 1 + CANCEL = 2 + SET_SUCCEEDED = 3 + SET_ABORTED = 4 + SET_CANCELED = 5 + + +class ServerGoalHandle: + """Goal handle for working with Action Servers.""" + + def __init__(self, action_server, goal_info, goal_request): + """ + Accept a new goal with the given action server. + + Instances of this class should only be constructed in the ActionServer class. + Instances for accepted goals will be passed to the user-defined goal execution functions. + + If the goal fails to be accepted, then a RuntimeError is raised. + + :param action_server: The ActionServer to accept the goal. + :param goal_info: GoalInfo message. + :param goal_request: The original goal request message from an ActionClient. + """ + self._handle = _rclpy_action.rclpy_action_accept_new_goal( + action_server._handle, goal_info) + self._action_server = action_server + self._goal_info = goal_info + self._goal_request = goal_request + self._cancel_requested = False + self._result_future = Future() + action_server.add_future(self._result_future) + self._lock = threading.Lock() + + def __eq__(self, other): + return self.goal_id == other.goal_id + + def __ne__(self, other): + return self.goal_id != other.goal_id + + @property + def request(self): + return self._goal_request + + @property + def goal_id(self): + return self._goal_info.goal_id + + @property + def is_active(self): + with self._lock: + if self._handle is None: + return False + return _rclpy_action.rclpy_action_goal_handle_is_active(self._handle) + + @property + def is_cancel_requested(self): + return GoalStatus.STATUS_CANCELING == self.status + + @property + def status(self): + with self._lock: + if self._handle is None: + return GoalStatus.STATUS_UNKNOWN + return _rclpy_action.rclpy_action_goal_handle_get_status(self._handle) + + def _update_state(self, event): + with self._lock: + # Ignore updates for already destructed goal handles + if self._handle is None: + return + + # Update state + _rclpy_action.rclpy_action_update_goal_state(self._handle, event.value) + + # Publish state change + _rclpy_action.rclpy_action_publish_status(self._action_server._handle) + + # If it's a terminal state, then also notify the action server + if not _rclpy_action.rclpy_action_goal_handle_is_active(self._handle): + self._action_server.notify_goal_done() + + def execute(self, execute_callback=None): + # It's possible that there has been a request to cancel the goal prior to executing. + # In this case we want to avoid the illegal state transition to EXECUTING + # but still call the users execute callback to let them handle canceling the goal. + if not self.is_cancel_requested: + self._update_state(GoalEvent.EXECUTE) + self._action_server.notify_execute(self, execute_callback) + + def publish_feedback(self, feedback_msg): + with self._lock: + # Ignore for already destructed goal handles + if self._handle is None: + return + + # Ensure the feedback message contains metadata about this goal + feedback_msg.action_goal_id = self.goal_id + + # Publish + _rclpy_action.rclpy_action_publish_feedback(self._action_server._handle, feedback_msg) + + def set_succeeded(self): + self._update_state(GoalEvent.SET_SUCCEEDED) + + def set_aborted(self): + self._update_state(GoalEvent.SET_ABORTED) + + def set_canceled(self): + self._update_state(GoalEvent.SET_CANCELED) + + def destroy(self): + with self._lock: + if self._handle is None: + return + _rclpy_action.rclpy_action_destroy_server_goal_handle(self._handle) + self._handle = None + + self._action_server.remove_future(self._result_future) + + +def default_handle_accepted_callback(goal_handle): + """Execute the goal.""" + goal_handle.execute() + + +def default_goal_callback(goal_request): + """Accept all goals.""" + return GoalResponse.ACCEPT + + +def default_cancel_callback(cancel_request): + """No cancellations.""" + return CancelResponse.REJECT + + +class ActionServer(Waitable): + """ROS Action server.""" + + def __init__( + self, + node, + action_type, + action_name, + execute_callback, + *, + callback_group=None, + goal_callback=default_goal_callback, + handle_accepted_callback=default_handle_accepted_callback, + cancel_callback=default_cancel_callback, + goal_service_qos_profile=qos_profile_services_default, + result_service_qos_profile=qos_profile_services_default, + cancel_service_qos_profile=qos_profile_services_default, + feedback_pub_qos_profile=qos_profile_default, + status_pub_qos_profile=qos_profile_action_status_default, + result_timeout=900 + ): + """ + Constructor. + + :param node: The ROS node to add the action server to. + :param action_type: Type of the action. + :param action_name: Name of the action. + Used as part of the underlying topic and service names. + :param execute_callback: Callback function for processing accepted goals. + This is called if when :class:`ServerGoalHandle.execute()` is called for + a goal handle that is being tracked by this action server. + :param callback_group: Callback group to add the action server to. + If None, then the node's default callback group is used. + :param goal_callback: Callback function for handling new goal requests. + :param handle_accepted_callback: Callback function for handling newly accepted goals. + Passes an instance of `ServerGoalHandle` as an argument. + :param cancel_callback: Callback function for handling cancel requests. + :param goal_service_qos_profile: QoS profile for the goal service. + :param result_service_qos_profile: QoS profile for the result service. + :param cancel_service_qos_profile: QoS profile for the cancel service. + :param feedback_pub_qos_profile: QoS profile for the feedback publisher. + :param status_pub_qos_profile: QoS profile for the status publisher. + :param result_timeout: How long in seconds a result is kept by the server after a goal + reaches a terminal state. + """ + if callback_group is None: + callback_group = node.default_callback_group + + super().__init__(callback_group) + + self._lock = threading.Lock() + + self.register_handle_accepted_callback(handle_accepted_callback) + self.register_goal_callback(goal_callback) + self.register_cancel_callback(cancel_callback) + self.register_execute_callback(execute_callback) + + # Import the typesupport for the action module if not already done + check_for_type_support(action_type) + self._node = node + self._action_type = action_type + self._handle = _rclpy_action.rclpy_action_create_server( + node.handle, + node.get_clock().handle, + action_type, + action_name, + goal_service_qos_profile.get_c_qos_profile(), + result_service_qos_profile.get_c_qos_profile(), + cancel_service_qos_profile.get_c_qos_profile(), + feedback_pub_qos_profile.get_c_qos_profile(), + status_pub_qos_profile.get_c_qos_profile(), + result_timeout, + ) + + # key: UUID in bytes, value: GoalHandle + self._goal_handles = {} + + callback_group.add_entity(self) + self._node.add_waitable(self) + + async def _execute_goal_request(self, request_header_and_message): + request_header, goal_request = request_header_and_message + goal_uuid = goal_request.action_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 = _rclpy_action.rclpy_action_server_goal_exists(self._handle, goal_info) + + accepted = False + if not goal_id_exists: + # Call user goal callback + response = await await_or_execute(self._goal_callback, goal_request) + if not isinstance(response, GoalResponse): + self._node.get_logger().warn( + '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) + 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.GoalRequestService.Response() + response_msg.accepted = accepted + response_msg.stamp = goal_info.stamp + _rclpy_action.rclpy_action_send_goal_response( + self._handle, + 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) + + async def _execute_goal(self, execute_callback, goal_handle): + goal_uuid = goal_handle.goal_id.uuid + self._node.get_logger().debug('Executing goal with ID {0}'.format(goal_uuid)) + + # Execute user callback + execute_result = await await_or_execute(execute_callback, goal_handle) + + # If user did not trigger a terminal state, assume aborted + if goal_handle.is_active: + self._node.get_logger().warn( + 'Goal state not set, assuming aborted. Goal ID: {0}'.format(goal_uuid)) + goal_handle.set_aborted() + + self._node.get_logger().debug( + 'Goal with ID {0} finished with state {1}'.format(goal_uuid, goal_handle.status)) + + # Set result + execute_result.action_status = goal_handle.status + goal_handle._result_future.set_result(execute_result) + + async def _execute_cancel_request(self, request_header_and_message): + request_header, cancel_request = request_header_and_message + + self._node.get_logger().debug('Cancel request received: {0}'.format(cancel_request)) + + with self._lock: + # Get list of goals that are requested to be canceled + cancel_response = _rclpy_action.rclpy_action_process_cancel_request( + self._handle, cancel_request, self._action_type.CancelGoalService.Response) + + for goal_info in cancel_response.goals_canceling: + goal_uuid = bytes(goal_info.goal_id.uuid) + if goal_uuid not in self._goal_handles: + # Possibly the user doesn't care to track the goal handle + # Remove from response + cancel_response.goals_canceling.remove(goal_info) + continue + goal_handle = self._goal_handles[goal_uuid] + response = await await_or_execute(self._cancel_callback, goal_handle) + + if CancelResponse.ACCEPT == response: + # Notify goal handle + goal_handle._update_state(GoalEvent.CANCEL) + else: + # Remove from response + cancel_response.goals_canceling.remove(goal_info) + + _rclpy_action.rclpy_action_send_cancel_response( + self._handle, + request_header, + cancel_response, + ) + + async def _execute_get_result_request(self, request_header_and_message): + request_header, result_request = request_header_and_message + goal_uuid = result_request.action_goal_id.uuid + + self._node.get_logger().debug( + 'Result request received for goal with ID: {0}'.format(goal_uuid)) + + # If no goal with the requested ID exists, then return UNKNOWN status + if bytes(goal_uuid) not in self._goal_handles: + self._node.get_logger().debug( + 'Sending result response for unknown goal ID: {0}'.format(goal_uuid)) + result_response = self._action_type.Result() + result_response.status = GoalStatus.STATUS_UNKNOWN + _rclpy_action.rclpy_action_send_result_response( + self._handle, + request_header, + result_response, + ) + return + + # There is an accepted goal matching the goal ID, register a callback to send the + # response as soon as it's ready + self._goal_handles[bytes(goal_uuid)]._result_future.add_done_callback( + functools.partial(self._send_result_response, request_header)) + + async def _execute_expire_goals(self, expired_goals): + for goal in expired_goals: + goal_uuid = bytes(goal.goal_id.uuid) + del self._goal_handles[goal_uuid] + + def _send_result_response(self, request_header, future): + _rclpy_action.rclpy_action_send_result_response( + self._handle, + request_header, + future.result(), + ) + + # Start Waitable API + def is_ready(self, wait_set): + """Return True if one or more entities are ready in the wait set.""" + with self._lock: + ready_entities = _rclpy_action.rclpy_action_wait_set_is_ready(self._handle, wait_set) + self._is_goal_request_ready = ready_entities[0] + self._is_cancel_request_ready = ready_entities[1] + self._is_result_request_ready = ready_entities[2] + self._is_goal_expired = ready_entities[3] + return any(ready_entities) + + def take_data(self): + """Take stuff from lower level so the wait set doesn't immediately wake again.""" + data = {} + if self._is_goal_request_ready: + with self._lock: + data['goal'] = _rclpy_action.rclpy_action_take_goal_request( + self._handle, + self._action_type.GoalRequestService.Request, + ) + + if self._is_cancel_request_ready: + with self._lock: + data['cancel'] = _rclpy_action.rclpy_action_take_cancel_request( + self._handle, + self._action_type.CancelGoalService.Request, + ) + + if self._is_result_request_ready: + with self._lock: + data['result'] = _rclpy_action.rclpy_action_take_result_request( + self._handle, + self._action_type.GoalResultService.Request, + ) + + if self._is_goal_expired: + with self._lock: + data['expired'] = _rclpy_action.rclpy_action_expire_goals( + self._handle, + len(self._goal_handles), + ) + + if not data: + return None + return data + + async def execute(self, taken_data): + """ + Execute work after data has been taken from a ready wait set. + + This will set results for Future objects for any received service responses and + call any user-defined callbacks (e.g. feedback). + """ + if 'goal' in taken_data: + await self._execute_goal_request(taken_data['goal']) + + if 'cancel' in taken_data: + await self._execute_cancel_request(taken_data['cancel']) + + if 'result' in taken_data: + await self._execute_get_result_request(taken_data['result']) + + if 'expired' in taken_data: + await self._execute_expire_goals(taken_data['expired']) + + def get_num_entities(self): + """Return number of each type of entity used in the wait set.""" + num_entities = _rclpy_action.rclpy_action_wait_set_get_num_entities(self._handle) + return NumberOfEntities( + num_entities[0], + num_entities[1], + num_entities[2], + num_entities[3], + num_entities[4]) + + def add_to_wait_set(self, wait_set): + """Add entities to wait set.""" + with self._lock: + _rclpy_action.rclpy_action_wait_set_add(self._handle, wait_set) + # End Waitable API + + def notify_execute(self, goal_handle, execute_callback): + # Use provided callback, defaulting to a previously registered callback + if execute_callback is None: + if self._execute_callback is None: + return + execute_callback = self._execute_callback + + # Schedule user callback for execution + self._node.executor.create_task(self._execute_goal, execute_callback, goal_handle) + + def notify_goal_done(self): + with self._lock: + _rclpy_action.rclpy_action_notify_goal_done(self._handle) + + def register_handle_accepted_callback(self, handle_accepted_callback): + """ + Register a callback for handling newly accepted goals. + + The provided function is called whenever a new goal has been accepted by this + action server. + The function should expect an instance of :class:`ServerGoalHandle` as an argument, which + represents a handle to the goal that was accepted. + The goal handle can be used to interact with the goal, e.g. publish feedback, + update the status, or execute a deferred goal. + + There can only be one handle accepted callback per :class:`ActionServer`, therefore + calling this function will replace any previously registered callback. + + :param goal_callback: Callback function, if `None`, then unregisters any previously + registered callback. + """ + if handle_accepted_callback is None: + handle_accepted_callback = default_handle_accepted_callback + self._handle_accepted_callback = handle_accepted_callback + + def register_goal_callback(self, goal_callback): + """ + Register a callback for handling new goal requests. + + The purpose of the goal callback is to decide if a new goal should be accepted or + rejected. + The callback should take the goal request message as a parameter and must return a + :class:`GoalResponse` value. + + There can only be one goal callback per :class:`ActionServer`, therefore calling this + function will replace any previously registered callback. + + :param goal_callback: Callback function, if `None`, then unregisters any previously + registered callback. + """ + if goal_callback is None: + goal_callback = default_goal_callback + self._goal_callback = goal_callback + + def register_cancel_callback(self, cancel_callback): + """ + Register a callback for handling cancel requests. + + The purpose of the cancel callback is to decide if a request to cancel an on-going + (or queued) goal should be accepted or rejected. + The callback should take one parameter containing the cancel request and must return a + :class:`CancelResponse` value. + + There can only be one cancel callback per :class:`ActionServer`, therefore calling this + function will replace any previously registered callback. + + :param cancel_callback: Callback function, if `None`, then unregisters any previously + registered callback. + """ + if cancel_callback is None: + cancel_callback = default_cancel_callback + self._cancel_callback = cancel_callback + + def register_execute_callback(self, execute_callback): + """ + Register a callback for executing action goals. + + The purpose of the execute callback is to execute the action goal and return a result + when finished. + + The callback should take one parameter containing goal request and must return a + result instance (i.e. `action_type.Result`). + + There can only be one execute callback per :class:`ActionServer`, therefore calling this + function will replace any previously registered callback. + + :param execute_callback: Callback function. Must not be `None`. + """ + if not callable(execute_callback): + raise TypeError('Failed to register goal execution callback: not callable') + self._execute_callback = execute_callback + + def destroy(self): + """Destroy the underlying action server handle.""" + if self._handle is None or self._node.handle is None: + return + + for goal_handle in self._goal_handles.values(): + goal_handle.destroy() + + _rclpy_action.rclpy_action_destroy_entity(self._handle, self._node.handle) + self._node.remove_waitable(self) + self._handle = None + + def __del__(self): + """Destroy the underlying action server handle.""" + self.destroy() diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index 78cb51c84..d9abdc881 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -132,6 +132,10 @@ def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): def clock_type(self): return self._clock_type + @property + def handle(self): + return self._clock_handle + def __repr__(self): return 'Clock(clock_type={0})'.format(self.clock_type.name) diff --git a/rclpy/src/rclpy/_rclpy_action.c b/rclpy/src/rclpy/_rclpy_action.c index a05df1275..7119c273a 100644 --- a/rclpy/src/rclpy/_rclpy_action.c +++ b/rclpy/src/rclpy/_rclpy_action.c @@ -76,6 +76,30 @@ rclpy_action_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args) Py_RETURN_NONE; } +static PyObject * +rclpy_action_destroy_server_goal_handle(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pygoal_handle; + + if (!PyArg_ParseTuple(args, "O", &pygoal_handle)) { + return NULL; + } + + rcl_action_goal_handle_t * goal_handle = (rcl_action_goal_handle_t *)PyCapsule_GetPointer( + pygoal_handle, "rcl_action_goal_handle_t"); + if (!goal_handle) { + return NULL; + } + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, "Error destroying action goal handle: %s", rcl_get_error_string().str); + rcl_reset_error(); + } + Py_RETURN_NONE; +} + /// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. /** * Raises RuntimeError if the QoS profile is unknown. @@ -503,6 +527,141 @@ rclpy_action_create_client(PyObject * Py_UNUSED(self), PyObject * args) return PyCapsule_New(action_client, "rcl_action_client_t", NULL); } +/// Create an action server. +/** + * This function will create an action server for the given action name. + * This server will use the typesupport defined in the action module + * provided as pyaction_type to send messages over the wire. + * + * On a successful call a capsule referencing the created rcl_action_server_t structure + * is returned. + * + * Raises AttributeError if action type is invalid + * Raises ValueError if action name is invalid + * Raises RuntimeError if the action server could not be created. + * + * \remark Call rclpy_action_destroy_entity() to destroy an action server. + * \param[in] pynode Capsule pointing to the node to add the action server to. + * \param[in] pyaction_type Action module associated with the action server. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] pygoal_service_qos Capsule pointing to a rmw_qos_profile_t object + * for the goal service. + * \param[in] pyresult_service_qos Capsule pointing to a rmw_qos_profile_t object + * for the result service. + * \param[in] pycancel_service_qos Capsule pointing to a rmw_qos_profile_t object + * for the cancel service. + * \param[in] pyfeedback_qos Capsule pointing to a rmw_qos_profile_t object + * for the feedback subscriber. + * \param[in] pystatus_qos Capsule pointing to a rmw_qos_profile_t object for the + * status subscriber. + * \return Capsule named 'rcl_action_server_t', or + * \return NULL on failure. + */ +static PyObject * +rclpy_action_create_server(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pynode; + PyObject * pyclock; + PyObject * pyaction_type; + PyObject * pyaction_name; + PyObject * pygoal_service_qos; + PyObject * pyresult_service_qos; + PyObject * pycancel_service_qos; + PyObject * pyfeedback_topic_qos; + PyObject * pystatus_topic_qos; + double result_timeout = 0.0; + + int parse_tuple_result = PyArg_ParseTuple( + args, + "OOOOOOOOOd", + &pynode, + &pyclock, + &pyaction_type, + &pyaction_name, + &pygoal_service_qos, + &pyresult_service_qos, + &pycancel_service_qos, + &pyfeedback_topic_qos, + &pystatus_topic_qos, + &result_timeout); + + if (!parse_tuple_result) { + return NULL; + } + + const char * action_name = PyUnicode_AsUTF8(pyaction_name); + if (!action_name) { + return NULL; + } + + rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t"); + if (!node) { + return NULL; + } + + rcl_clock_t * clock = (rcl_clock_t *) PyCapsule_GetPointer(pyclock, "rcl_clock_t"); + if (!clock) { + return NULL; + } + + PyObject * pymetaclass = PyObject_GetAttrString(pyaction_type, "__class__"); + if (!pymetaclass) { + return NULL; + } + + PyObject * pyts = PyObject_GetAttrString(pymetaclass, "_TYPE_SUPPORT"); + Py_DECREF(pymetaclass); + if (!pyts) { + return NULL; + } + + rosidl_action_type_support_t * ts = + (rosidl_action_type_support_t *)PyCapsule_GetPointer(pyts, NULL); + Py_DECREF(pyts); + if (!ts) { + return NULL; + } + + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + OPTIONS_COPY_QOS_PROFILE(action_server_ops, goal_service_qos); + OPTIONS_COPY_QOS_PROFILE(action_server_ops, result_service_qos); + OPTIONS_COPY_QOS_PROFILE(action_server_ops, cancel_service_qos); + OPTIONS_COPY_QOS_PROFILE(action_server_ops, feedback_topic_qos); + OPTIONS_COPY_QOS_PROFILE(action_server_ops, status_topic_qos); + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); + + rcl_action_server_t * action_server = + (rcl_action_server_t *)PyMem_Malloc(sizeof(rcl_action_server_t)); + if (!action_server) { + return PyErr_NoMemory(); + } + *action_server = rcl_action_get_zero_initialized_server(); + rcl_ret_t ret = rcl_action_server_init( + action_server, + node, + clock, + ts, + action_name, + &action_server_ops); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_ACTION_NAME_INVALID) { + PyErr_Format(PyExc_ValueError, + "Failed to create action server due to invalid topic name '%s': %s", + action_name, rcl_get_error_string().str); + } else { + PyErr_Format(PyExc_RuntimeError, + "Failed to create action server: %s", rcl_get_error_string().str); + } + PyMem_Free(action_server); + rcl_reset_error(); + return NULL; + } + + return PyCapsule_New(action_server, "rcl_action_server_t", NULL); +} + + /// Check if an action server is available for the given action client. /** * Raises RuntimeError on failure. @@ -555,35 +714,9 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) if (!action_client) { \ return NULL; \ } \ - PyObject * pyrequest_type = PyObject_GetAttrString(pyrequest, "__class__"); \ - if (!pyrequest_type) { \ - return NULL; \ - } \ - PyObject * pymetaclass = PyObject_GetAttrString(pyrequest_type, "__class__"); \ - Py_DECREF(pyrequest_type); \ - if (!pymetaclass) { \ - return NULL; \ - } \ - create_ros_message_signature * create_ros_message = get_capsule_pointer( \ - pymetaclass, "_CREATE_ROS_MESSAGE"); \ - assert(create_ros_message != NULL && \ - "unable to retrieve create_ros_message function, type_support must not have been imported"); \ - destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \ - pymetaclass, "_DESTROY_ROS_MESSAGE"); \ - assert(destroy_ros_message != NULL && \ - "unable to retrieve destroy_ros_message function, type_support must not have been imported"); \ - convert_from_py_signature * convert_from_py = get_capsule_pointer( \ - pymetaclass, "_CONVERT_FROM_PY"); \ - assert(convert_from_py != NULL && \ - "unable to retrieve convert_from_py function, type_support must not have been imported"); \ - Py_DECREF(pymetaclass); \ - void * raw_ros_request = create_ros_message(); \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * raw_ros_request = rclpy_convert_from_py(pyrequest, & destroy_ros_message); \ if (!raw_ros_request) { \ - return PyErr_NoMemory(); \ - } \ - if (!convert_from_py(pyrequest, raw_ros_request)) { \ - /* the function has set the Python error */ \ - destroy_ros_message(raw_ros_request); \ return NULL; \ } \ int64_t sequence_number; \ @@ -598,10 +731,105 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) } \ return PyLong_FromLongLong(sequence_number); +#define SEND_SERVICE_RESPONSE(Type) \ + PyObject * pyaction_server; \ + PyObject * pyheader; \ + PyObject * pyresponse; \ + if (!PyArg_ParseTuple(args, "OOO", & pyaction_server, & pyheader, & pyresponse)) { \ + return NULL; \ + } \ + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( \ + pyaction_server, "rcl_action_server_t"); \ + if (!action_server) { \ + return NULL; \ + } \ + rmw_request_id_t * header = (rmw_request_id_t *)PyCapsule_GetPointer( \ + pyheader, "rmw_request_id_t"); \ + if (!header) { \ + return NULL; \ + } \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * raw_ros_response = rclpy_convert_from_py(pyresponse, & destroy_ros_message); \ + if (!raw_ros_response) { \ + return NULL; \ + } \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response(action_server, header, raw_ros_response); \ + destroy_ros_message(raw_ros_response); \ + if (ret != RCL_RET_OK) { \ + PyErr_Format(PyExc_RuntimeError, \ + "Failed to send " #Type " response: %s", rcl_get_error_string().str); \ + rcl_reset_error(); \ + return NULL; \ + } \ + Py_RETURN_NONE; + +#define TAKE_SERVICE_REQUEST(Type) \ + PyObject * pyaction_server; \ + PyObject * pymsg_type; \ + if (!PyArg_ParseTuple(args, "OO", & pyaction_server, & pymsg_type)) { \ + return NULL; \ + } \ + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( \ + pyaction_server, "rcl_action_server_t"); \ + if (!action_server) { \ + return NULL; \ + } \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * taken_msg = rclpy_create_from_py(pymsg_type, & destroy_ros_message); \ + if (!taken_msg) { \ + /* the function has set the Python error */ \ + return NULL; \ + } \ + rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); \ + if (!header) { \ + destroy_ros_message(taken_msg); \ + return PyErr_NoMemory(); \ + } \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _request(action_server, header, taken_msg); \ + /* Create the tuple to return */ \ + PyObject * pytuple = PyTuple_New(2); \ + if (!pytuple) { \ + destroy_ros_message(taken_msg); \ + PyMem_Free(header); \ + return NULL; \ + } \ + if (ret != RCL_RET_OK) { \ + Py_INCREF(Py_None); \ + PyTuple_SET_ITEM(pytuple, 0, Py_None); \ + Py_INCREF(Py_None); \ + PyTuple_SET_ITEM(pytuple, 1, Py_None); \ + destroy_ros_message(taken_msg); \ + PyMem_Free(header); \ + if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED && ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + PyErr_Format(PyExc_RuntimeError, \ + "Failed to take " #Type ": %s", rcl_get_error_string().str); \ + rcl_reset_error(); \ + return NULL; \ + } \ + return pytuple; \ + } \ + PyObject * pytaken_msg = rclpy_convert_to_py(taken_msg, pymsg_type); \ + destroy_ros_message(taken_msg); \ + if (!pytaken_msg) { \ + Py_DECREF(pytuple); \ + PyMem_Free(header); \ + return NULL; \ + } \ + PyObject * pyheader = PyCapsule_New(header, "rmw_request_id_t", NULL); \ + if (!pyheader) { \ + Py_DECREF(pytaken_msg); \ + Py_DECREF(pytuple); \ + PyMem_Free(header); \ + return NULL; \ + } \ + PyTuple_SET_ITEM(pytuple, 0, pyheader); \ + PyTuple_SET_ITEM(pytuple, 1, pytaken_msg); \ + return pytuple; \ + #define TAKE_SERVICE_RESPONSE(Type) \ PyObject * pyaction_client; \ - PyObject * pyresponse_type; \ - if (!PyArg_ParseTuple(args, "OO", & pyaction_client, & pyresponse_type)) { \ + PyObject * pymsg_type; \ + if (!PyArg_ParseTuple(args, "OO", & pyaction_client, & pymsg_type)) { \ return NULL; \ } \ rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer( \ @@ -609,36 +837,23 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) if (!action_client) { \ return NULL; \ } \ - PyObject * pymetaclass = PyObject_GetAttrString(pyresponse_type, "__class__"); \ - if (!pymetaclass) { \ - return NULL; \ - } \ - create_ros_message_signature * create_ros_message = get_capsule_pointer( \ - pymetaclass, "_CREATE_ROS_MESSAGE"); \ - assert(create_ros_message != NULL && \ - "unable to retrieve create_ros_message function, type_support mustn't have been imported"); \ - destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \ - pymetaclass, "_DESTROY_ROS_MESSAGE"); \ - assert(destroy_ros_message != NULL && \ - "unable to retrieve destroy_ros_message function, type_support mustn't have been imported"); \ - void * taken_response = create_ros_message(); \ - if (!taken_response) { \ - /* the function has set the Python error */ \ - Py_DECREF(pymetaclass); \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * taken_msg = rclpy_create_from_py(pymsg_type, & destroy_ros_message); \ + if (!taken_msg) { \ return NULL; \ } \ rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); \ if (!header) { \ - Py_DECREF(pymetaclass); \ + destroy_ros_message(taken_msg); \ return PyErr_NoMemory(); \ } \ - rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, header, taken_response); \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, header, taken_msg); \ int64_t sequence = header->sequence_number; \ PyMem_Free(header); \ /* Create the tuple to return */ \ PyObject * pytuple = PyTuple_New(2); \ if (!pytuple) { \ - Py_DECREF(pymetaclass); \ + destroy_ros_message(taken_msg); \ return NULL; \ } \ if (ret != RCL_RET_OK) { \ @@ -646,32 +861,35 @@ rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args) PyTuple_SET_ITEM(pytuple, 0, Py_None); \ Py_INCREF(Py_None); \ PyTuple_SET_ITEM(pytuple, 1, Py_None); \ - Py_DECREF(pymetaclass); \ - destroy_ros_message(taken_response); \ + destroy_ros_message(taken_msg); \ + if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED && ret != RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + PyErr_Format(PyExc_RuntimeError, \ + "Failed to take " #Type ": %s", rcl_get_error_string().str); \ + rcl_reset_error(); \ + return NULL; \ + } \ return pytuple; \ } \ - convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY"); \ - Py_DECREF(pymetaclass); \ - PyObject * pytaken_response = convert_to_py(taken_response); \ - destroy_ros_message(taken_response); \ - if (!pytaken_response) { \ - /* the function has set the Python error */ \ + PyObject * pytaken_msg = rclpy_convert_to_py(taken_msg, pymsg_type); \ + destroy_ros_message(taken_msg); \ + if (!pytaken_msg) { \ Py_DECREF(pytuple); \ return NULL; \ } \ PyObject * pysequence = PyLong_FromLongLong(sequence); \ if (!pysequence) { \ - Py_DECREF(pytaken_response); \ + Py_DECREF(pytaken_msg); \ Py_DECREF(pytuple); \ return NULL; \ } \ PyTuple_SET_ITEM(pytuple, 0, pysequence); \ - PyTuple_SET_ITEM(pytuple, 1, pytaken_response); \ + PyTuple_SET_ITEM(pytuple, 1, pytaken_msg); \ return pytuple; \ + /// Send an action goal request. /** - * Raises AttributeError if there is an issue parsing the pygoal_request type. + * Raises AttributeError if there is an issue parsing the pygoal_request. * Raises RuntimeError on failure. * * \param[in] pyaction_client The action client to use when sending the request. @@ -685,15 +903,50 @@ rclpy_action_send_goal_request(PyObject * Py_UNUSED(self), PyObject * args) SEND_SERVICE_REQUEST(goal) } +/// Take an action goal request. +/** + * Raises AttributeError if there is an issue parsing the pygoal_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when taking the request. + * \param[in] pygoal_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is a Capsule of + * type "rmw_request_id_t", or + * \return 2-tuple (None, None) if there as no message to take, or + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_take_goal_request(PyObject * Py_UNUSED(self), PyObject * args) +{ + TAKE_SERVICE_REQUEST(goal) +} + +/// Send an action goal response. +/** + * Raises AttributeError if there is an issue parsing the pygoal_response. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when sending the response. + * \param[in] pyheader Capsule pointer to the message header of type "rmw_request_id_t". + * \param[in] pygoal_response The response message to send. + * \return None + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_send_goal_response(PyObject * Py_UNUSED(self), PyObject * args) +{ + SEND_SERVICE_RESPONSE(goal) +} + /// Take an action goal response. /** - * Raises AttributeError if there is an issue parsing the pygoal_response type. + * Raises AttributeError if there is an issue parsing the pygoal_response_type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. * \param[in] pygoal_response_type An instance of the response message type to take. - * \return 2-tuple sequence number and received response, or - * \return None if there is no response, or + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or * \return NULL if there is a failure. */ static PyObject * @@ -704,7 +957,7 @@ rclpy_action_take_goal_response(PyObject * Py_UNUSED(self), PyObject * args) /// Send an action result request. /** - * Raises AttributeError if there is an issue parsing the pyresult_request type. + * Raises AttributeError if there is an issue parsing the pyresult_request. * Raises RuntimeError if the underlying rcl library returns an error when sending the request. * * \param[in] pyaction_client The action client to use when sending the request. @@ -718,15 +971,50 @@ rclpy_action_send_result_request(PyObject * Py_UNUSED(self), PyObject * args) SEND_SERVICE_REQUEST(result); } +/// Take an action result request. +/** + * Raises AttributeError if there is an issue parsing the pyresult_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when taking the request. + * \param[in] pyresult_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is a Capsule of + * type "rmw_request_id_t", or + * \return 2-tuple (None, None) if there as no message to take, or + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_take_result_request(PyObject * Py_UNUSED(self), PyObject * args) +{ + TAKE_SERVICE_REQUEST(result) +} + +/// Send an action result response. +/** + * Raises AttributeError if there is an issue parsing the pyresult_response. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when sending the response. + * \param[in] pyheader Capsule pointer to the message header of type "rmw_request_id_t". + * \param[in] pyresult_response The response message to send. + * \return None + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_send_result_response(PyObject * Py_UNUSED(self), PyObject * args) +{ + SEND_SERVICE_RESPONSE(result) +} + /// Take an action result response. /** - * Raises AttributeError if there is an issue parsing the pyresult_response type. + * Raises AttributeError if there is an issue parsing the pyresult_response_type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. * \param[in] pyresult_response_type An instance of the response message type to take. - * \return 2-tuple sequence number and received response, or - * \return None if there is no response, or + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or * \return NULL if there is a failure. */ static PyObject * @@ -737,7 +1025,7 @@ rclpy_action_take_result_response(PyObject * Py_UNUSED(self), PyObject * args) /// Send an action cancel request. /** - * Raises AttributeError if there is an issue parsing the pycancel_request type. + * Raises AttributeError if there is an issue parsing the pycancel_request. * Raises RuntimeError if the underlying rcl library returns an error when sending the request. * * \param[in] pyaction_client The action client to use when sending the request. @@ -751,15 +1039,50 @@ rclpy_action_send_cancel_request(PyObject * Py_UNUSED(self), PyObject * args) SEND_SERVICE_REQUEST(cancel) } +/// Take an action cancel request. +/** + * Raises AttributeError if there is an issue parsing the pycancel_request_type. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when taking the request. + * \param[in] pycancel_request_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is a Capsule of + * type "rmw_request_id_t", or + * \return 2-tuple (None, None) if there as no message to take, or + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_take_cancel_request(PyObject * Py_UNUSED(self), PyObject * args) +{ + TAKE_SERVICE_REQUEST(cancel) +} + +/// Send an action cancel response. +/** + * Raises AttributeError if there is an issue parsing the pycancel_response. + * Raises RuntimeError on failure. + * + * \param[in] pyaction_server The action server to use when sending the response. + * \param[in] pyheader Capsule pointer to the message header of type "rmw_request_id_t". + * \param[in] pycancel_response The response message to send. + * \return sequence_number PyLong object representing the index of the sent response, or + * \return NULL if there is a failure. + */ +static PyObject * +rclpy_action_send_cancel_response(PyObject * Py_UNUSED(self), PyObject * args) +{ + SEND_SERVICE_RESPONSE(cancel) +} + /// Take an action cancel response. /** - * Raises AttributeError if there is an issue parsing the pycancel_response type. + * Raises AttributeError if there is an issue parsing the pycancel_response_type. * Raises RuntimeError if the underlying rcl library returns an error when taking the response. * * \param[in] pyaction_client The action client to use when sending the request. * \param[in] pycancel_response_type An instance of the response message type to take. - * \return 2-tuple sequence number and received response, or - * \return None if there is no response, or + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or * \return NULL if there is a failure. */ static PyObject * @@ -768,6 +1091,32 @@ rclpy_action_take_cancel_response(PyObject * Py_UNUSED(self), PyObject * args) TAKE_SERVICE_RESPONSE(cancel) } +#define PUBLISH_MESSAGE(Type) \ + PyObject * pyaction_server; \ + PyObject * pymsg; \ + if (!PyArg_ParseTuple(args, "OO", & pyaction_server, & pymsg)) { \ + return NULL; \ + } \ + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( \ + pyaction_server, "rcl_action_server_t"); \ + if (!action_server) { \ + return NULL; \ + } \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * raw_ros_message = rclpy_convert_from_py(pymsg, & destroy_ros_message); \ + if (!raw_ros_message) { \ + return NULL; \ + } \ + rcl_ret_t ret = rcl_action_publish_ ## Type(action_server, raw_ros_message); \ + destroy_ros_message(raw_ros_message); \ + if (ret != RCL_RET_OK) { \ + PyErr_Format(PyExc_RuntimeError, \ + "Failed to publish " #Type " with an action server: %s", rcl_get_error_string().str); \ + rcl_reset_error(); \ + return NULL; \ + } \ + Py_RETURN_NONE; + #define TAKE_MESSAGE(Type) \ PyObject * pyaction_client; \ PyObject * pymsg_type; \ @@ -779,41 +1128,42 @@ rclpy_action_take_cancel_response(PyObject * Py_UNUSED(self), PyObject * args) if (!action_client) { \ return NULL; \ } \ - PyObject * pymetaclass = PyObject_GetAttrString(pymsg_type, "__class__"); \ - if (!pymetaclass) { \ - return NULL; \ - } \ - create_ros_message_signature * create_ros_message = get_capsule_pointer( \ - pymetaclass, "_CREATE_ROS_MESSAGE"); \ - assert(create_ros_message != NULL && \ - "unable to retrieve create_ros_message function, type_support must not have been imported"); \ - destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \ - pymetaclass, "_DESTROY_ROS_MESSAGE"); \ - assert(destroy_ros_message != NULL && \ - "unable to retrieve destroy_ros_message function, type_support must not have been imported"); \ - convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY"); \ - Py_DECREF(pymetaclass); \ - void * taken_msg = create_ros_message(); \ + destroy_ros_message_signature * destroy_ros_message = NULL; \ + void * taken_msg = rclpy_create_from_py(pymsg_type, & destroy_ros_message); \ if (!taken_msg) { \ - return PyErr_NoMemory(); \ + return NULL; \ } \ rcl_ret_t ret = rcl_action_take_ ## Type(action_client, taken_msg); \ if (ret != RCL_RET_OK) { \ + destroy_ros_message(taken_msg); \ if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { \ /* if take failed, just do nothing */ \ - destroy_ros_message(taken_msg); \ Py_RETURN_NONE; \ } \ PyErr_Format(PyExc_RuntimeError, \ "Failed to take " #Type " with an action client: %s", rcl_get_error_string().str); \ rcl_reset_error(); \ - destroy_ros_message(taken_msg); \ return NULL; \ } \ - PyObject * pytaken_msg = convert_to_py(taken_msg); \ + PyObject * pytaken_msg = rclpy_convert_to_py(taken_msg, pymsg_type); \ destroy_ros_message(taken_msg); \ return pytaken_msg; +/// Publish a feedback message from a given action server. +/** + * Raises AttributeError if there is an issue parsing the pyfeedback_msg. + * Raises RuntimeError on failure while publishing a feedback message. + * + * \param[in] pyaction_server Capsule pointing to the action server to publish the message. + * \param[in] pyfeedback_msg The feedback message to publish. + * \return None + */ +static PyObject * +rclpy_action_publish_feedback(PyObject * Py_UNUSED(self), PyObject * args) +{ + PUBLISH_MESSAGE(feedback) +} + /// Take a feedback message from a given action client. /** * Raises AttributeError if there is an issue parsing the pyfeedback_type. @@ -832,6 +1182,54 @@ rclpy_action_take_feedback(PyObject * Py_UNUSED(self), PyObject * args) TAKE_MESSAGE(feedback) } +/// Publish a status message from a given action server. +/** + * Raises RuntimeError on failure while publishing a status message. + * + * \param[in] pyaction_server Capsule pointing to the action server to publish the message. + * \return None + */ +static PyObject * +rclpy_action_publish_status(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + + if (!PyArg_ParseTuple(args, "O", &pyaction_server)) { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, + "Failed get goal status array: %s", + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, + "Failed publish goal status array: %s", + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + Py_RETURN_NONE; +} + /// Take a status message from a given action client. /** * Raises AttributeError if there is an issue parsing the pystatus_type. @@ -850,12 +1248,452 @@ rclpy_action_take_status(PyObject * Py_UNUSED(self), PyObject * args) TAKE_MESSAGE(status) } +static PyObject * +rclpy_action_accept_new_goal(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + PyObject * pygoal_info_msg; + + if (!PyArg_ParseTuple(args, "OO", &pyaction_server, &pygoal_info_msg)) { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + destroy_ros_message_signature * destroy_ros_message = NULL; + rcl_action_goal_info_t * goal_info_msg = (rcl_action_goal_info_t *)rclpy_convert_from_py( + pygoal_info_msg, &destroy_ros_message); + if (!goal_info_msg) { + return NULL; + } + + rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( + action_server, goal_info_msg); + destroy_ros_message(goal_info_msg); + if (!goal_handle) { + PyErr_Format(PyExc_RuntimeError, "Failed to accept new goal: %s", rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + return PyCapsule_New(goal_handle, "rcl_action_goal_handle_t", NULL); +} + +static PyObject * +rclpy_action_notify_goal_done(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + + if (!PyArg_ParseTuple(args, "O", &pyaction_server)) { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, + "Failed to notfiy action server of goal done: %s", + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + Py_RETURN_NONE; +} + +#define MULTI_DECREF(Arr, Size) \ + for (size_t i = 0; i < Size; ++i) { \ + Py_DECREF(Arr[i]); \ + } + +/// Convert from a Python GoalEvent code to an rcl goal event code. +/** + * Note, this this function makes the assumption that no event code has the value -1. + * \param[in] pyevent The Python GoalEvent code. + * \return The rcl equivalent of the Python GoalEvent code, or + * \return -1 on failure. + */ +static int +convert_from_py_goal_event(const int64_t pyevent) +{ + // Holds references to PyObjects that should have references decremented + PyObject * to_decref[11]; + // The number of objects in the decref list + size_t num_to_decref = 0; + + PyObject * pyaction_server_module = PyImport_ImportModule("rclpy.action.server"); + if (!pyaction_server_module) { + return -1; + } + + PyObject * pygoal_event_class = PyObject_GetAttrString(pyaction_server_module, "GoalEvent"); + Py_DECREF(pyaction_server_module); + if (!pygoal_event_class) { + return -1; + } + to_decref[num_to_decref++] = pygoal_event_class; + + PyObject * pyexecute = PyObject_GetAttrString(pygoal_event_class, "EXECUTE"); + if (!pyexecute) { + MULTI_DECREF(to_decref, num_to_decref) + return -1; + } + to_decref[num_to_decref++] = pyexecute; + + PyObject * pycancel = PyObject_GetAttrString(pygoal_event_class, "CANCEL"); + if (!pycancel) { + MULTI_DECREF(to_decref, num_to_decref) + return -1; + } + to_decref[num_to_decref++] = pycancel; + + PyObject * pyset_succeeded = PyObject_GetAttrString(pygoal_event_class, "SET_SUCCEEDED"); + if (!pyset_succeeded) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pyset_succeeded; + + PyObject * pyset_aborted = PyObject_GetAttrString(pygoal_event_class, "SET_ABORTED"); + if (!pyset_aborted) { + MULTI_DECREF(to_decref, num_to_decref) + return -1; + } + to_decref[num_to_decref++] = pyset_aborted; + + PyObject * pyset_canceled = PyObject_GetAttrString(pygoal_event_class, "SET_CANCELED"); + if (!pyset_canceled) { + MULTI_DECREF(to_decref, num_to_decref) + return -1; + } + to_decref[num_to_decref++] = pyset_canceled; + + PyObject * pyexecute_val = PyObject_GetAttrString(pyexecute, "value"); + if (!pyexecute_val) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pyexecute_val; + + PyObject * pycancel_val = PyObject_GetAttrString(pycancel, "value"); + if (!pycancel_val) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pycancel_val; + + PyObject * pyset_succeeded_val = PyObject_GetAttrString(pyset_succeeded, "value"); + if (!pyset_succeeded_val) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pyset_succeeded_val; + + PyObject * pyset_aborted_val = PyObject_GetAttrString(pyset_aborted, "value"); + if (!pyset_aborted_val) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pyset_aborted_val; + + PyObject * pyset_canceled_val = PyObject_GetAttrString(pyset_canceled, "value"); + if (!pyset_canceled_val) { + MULTI_DECREF(to_decref, num_to_decref); + return -1; + } + to_decref[num_to_decref++] = pyset_canceled_val; + + const int64_t execute = PyLong_AsLong(pyexecute_val); + const int64_t cancel = PyLong_AsLong(pycancel_val); + const int64_t set_succeeded = PyLong_AsLong(pyset_succeeded_val); + const int64_t set_aborted = PyLong_AsLong(pyset_aborted_val); + const int64_t set_canceled = PyLong_AsLong(pyset_canceled_val); + MULTI_DECREF(to_decref, num_to_decref) + + if (execute == pyevent) { + return GOAL_EVENT_EXECUTE; + } + if (cancel == pyevent) { + return GOAL_EVENT_CANCEL; + } + if (set_succeeded == pyevent) { + return GOAL_EVENT_SET_SUCCEEDED; + } + if (set_aborted == pyevent) { + return GOAL_EVENT_SET_ABORTED; + } + if (set_canceled == pyevent) { + return GOAL_EVENT_SET_CANCELED; + } + + PyErr_Format( + PyExc_RuntimeError, "Error converting goal event type: unknown goal event '%d'", pyevent); + return -1; +} + +static PyObject * +rclpy_action_update_goal_state(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pygoal_handle; + int64_t pyevent; + + if (!PyArg_ParseTuple(args, "OL", &pygoal_handle, &pyevent)) { + return NULL; + } + + rcl_action_goal_handle_t * goal_handle = (rcl_action_goal_handle_t *)PyCapsule_GetPointer( + pygoal_handle, "rcl_action_goal_handle_t"); + if (!goal_handle) { + return NULL; + } + + int event = convert_from_py_goal_event(pyevent); + if (event < 0) { + return NULL; + } + + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, "Failed to update goal state: %s", rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + Py_RETURN_NONE; +} + +static PyObject * +rclpy_action_goal_handle_is_active(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pygoal_handle; + + if (!PyArg_ParseTuple(args, "O", &pygoal_handle)) { + return NULL; + } + + rcl_action_goal_handle_t * goal_handle = (rcl_action_goal_handle_t *)PyCapsule_GetPointer( + pygoal_handle, "rcl_action_goal_handle_t"); + if (!goal_handle) { + return NULL; + } + + bool is_active = rcl_action_goal_handle_is_active(goal_handle); + if (is_active) { + Py_RETURN_TRUE; + } + Py_RETURN_FALSE; +} + +static PyObject * +rclpy_action_server_goal_exists(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + PyObject * pygoal_info; + + if (!PyArg_ParseTuple(args, "OO", &pyaction_server, &pygoal_info)) { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + destroy_ros_message_signature * destroy_ros_message = NULL; + rcl_action_goal_info_t * goal_info = rclpy_convert_from_py(pygoal_info, &destroy_ros_message); + if (!goal_info) { + return NULL; + } + + bool exists = rcl_action_server_goal_exists(action_server, goal_info); + destroy_ros_message(goal_info); + + if (exists) { + Py_RETURN_TRUE; + } + Py_RETURN_FALSE; +} + +static PyObject * +rclpy_action_goal_handle_get_status(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pygoal_handle; + + if (!PyArg_ParseTuple(args, "O", &pygoal_handle)) { + return NULL; + } + + rcl_action_goal_handle_t * goal_handle = (rcl_action_goal_handle_t *)PyCapsule_GetPointer( + pygoal_handle, "rcl_action_goal_handle_t"); + if (!goal_handle) { + return NULL; + } + + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); + if (RCL_RET_OK != ret) { + PyErr_Format( + PyExc_RuntimeError, "Failed to get goal status: %s", rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + return PyLong_FromLong(status); +} + +static PyObject * +rclpy_action_process_cancel_request(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + PyObject * pycancel_request; + PyObject * pycancel_response_type; + + if (!PyArg_ParseTuple( + args, + "OOO", + &pyaction_server, + &pycancel_request, + &pycancel_response_type)) + { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + destroy_ros_message_signature * destroy_cancel_request = NULL; + rcl_action_cancel_request_t * cancel_request = + (rcl_action_cancel_request_t *)rclpy_convert_from_py(pycancel_request, &destroy_cancel_request); + if (!cancel_request) { + return NULL; + } + + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + action_server, cancel_request, &cancel_response); + destroy_cancel_request(cancel_request); + if (RCL_RET_OK != ret) { + ret = rcl_action_cancel_response_fini(&cancel_response); + PyErr_Format(PyExc_RuntimeError, + "Failed to process cancel request: %s", + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + + PyObject * pycancel_response = rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type); + ret = rcl_action_cancel_response_fini(&cancel_response); + if (!pycancel_response) { + return NULL; + } + if (RCL_RET_OK != ret) { + PyErr_Format(PyExc_RuntimeError, + "Failed to finalize cancel response: %s", + rcl_get_error_string().str); + rcl_reset_error(); + return NULL; + } + return pycancel_response; +} + +static PyObject * +rclpy_action_expire_goals(PyObject * Py_UNUSED(self), PyObject * args) +{ + PyObject * pyaction_server; + int64_t max_num_goals; + + if (!PyArg_ParseTuple(args, "OL", &pyaction_server, &max_num_goals)) { + return NULL; + } + + rcl_action_server_t * action_server = (rcl_action_server_t *)PyCapsule_GetPointer( + pyaction_server, "rcl_action_server_t"); + if (!action_server) { + return NULL; + } + + rcl_action_goal_info_t * expired_goals = + (rcl_action_goal_info_t *)malloc(sizeof(rcl_action_goal_info_t) * max_num_goals); + if (!expired_goals) { + return PyErr_NoMemory(); + } + size_t num_expired; + rcl_ret_t ret = rcl_action_expire_goals( + action_server, expired_goals, max_num_goals, &num_expired); + if (RCL_RET_OK != ret) { + PyErr_Format(PyExc_RuntimeError, "Failed to expire goals: %s", rcl_get_error_string().str); + rcl_reset_error(); + free(expired_goals); + return NULL; + } + + // Get Python GoalInfo type + PyObject * pyaction_msgs_module = PyImport_ImportModule("action_msgs.msg"); + if (!pyaction_msgs_module) { + free(expired_goals); + return NULL; + } + PyObject * pygoal_info_class = PyObject_GetAttrString(pyaction_msgs_module, "GoalInfo"); + Py_DECREF(pyaction_msgs_module); + if (!pygoal_info_class) { + free(expired_goals); + return NULL; + } + PyObject * pygoal_info_type = PyObject_CallObject(pygoal_info_class, NULL); + Py_DECREF(pygoal_info_class); + if (!pygoal_info_type) { + free(expired_goals); + return NULL; + } + + // Create a tuple of GoalInfo instances to return + PyObject * result_tuple = PyTuple_New(num_expired); + if (!result_tuple) { + free(expired_goals); + Py_DECREF(pygoal_info_type); + return NULL; + } + // PyTuple_SetItem() returns 0 on success + int set_result = 0; + for (size_t i = 0; i < num_expired; ++i) { + PyObject * pygoal_info = rclpy_convert_to_py(&(expired_goals[i]), pygoal_info_type); + set_result += PyTuple_SetItem(result_tuple, i, pygoal_info); + } + + free(expired_goals); + Py_DECREF(pygoal_info_type); + if (0 != set_result) { + Py_DECREF(result_tuple); + return NULL; + } + return result_tuple; +} + /// Define the public methods of this module static PyMethodDef rclpy_action_methods[] = { { "rclpy_action_destroy_entity", rclpy_action_destroy_entity, METH_VARARGS, "Destroy a rclpy_action entity." }, + { + "rclpy_action_destroy_server_goal_handle", + rclpy_action_destroy_server_goal_handle, + METH_VARARGS, + "Destroy a ServerGoalHandle." + }, { "rclpy_action_get_rmw_qos_profile", rclpy_action_get_rmw_qos_profile, METH_VARARGS, "Get an action RMW QoS profile." @@ -876,6 +1714,10 @@ static PyMethodDef rclpy_action_methods[] = { "rclpy_action_create_client", rclpy_action_create_client, METH_VARARGS, "Create an action client." }, + { + "rclpy_action_create_server", rclpy_action_create_server, METH_VARARGS, + "Create an action server." + }, { "rclpy_action_server_is_available", rclpy_action_server_is_available, METH_VARARGS, "Check if an action server is available for a given client." @@ -884,6 +1726,14 @@ static PyMethodDef rclpy_action_methods[] = { "rclpy_action_send_goal_request", rclpy_action_send_goal_request, METH_VARARGS, "Send a goal request." }, + { + "rclpy_action_take_goal_request", rclpy_action_take_goal_request, METH_VARARGS, + "Take a goal request." + }, + { + "rclpy_action_send_goal_response", rclpy_action_send_goal_response, METH_VARARGS, + "Send a goal response." + }, { "rclpy_action_take_goal_response", rclpy_action_take_goal_response, METH_VARARGS, "Take a goal response." @@ -892,6 +1742,14 @@ static PyMethodDef rclpy_action_methods[] = { "rclpy_action_send_result_request", rclpy_action_send_result_request, METH_VARARGS, "Send a result request." }, + { + "rclpy_action_take_result_request", rclpy_action_take_result_request, METH_VARARGS, + "Take a result request." + }, + { + "rclpy_action_send_result_response", rclpy_action_send_result_response, METH_VARARGS, + "Send a result response." + }, { "rclpy_action_take_result_response", rclpy_action_take_result_response, METH_VARARGS, "Take a result response." @@ -900,18 +1758,66 @@ static PyMethodDef rclpy_action_methods[] = { "rclpy_action_send_cancel_request", rclpy_action_send_cancel_request, METH_VARARGS, "Send a cancel request." }, + { + "rclpy_action_take_cancel_request", rclpy_action_take_cancel_request, METH_VARARGS, + "Take a cancel request." + }, + { + "rclpy_action_send_cancel_response", rclpy_action_send_cancel_response, METH_VARARGS, + "Send a cancel response." + }, { "rclpy_action_take_cancel_response", rclpy_action_take_cancel_response, METH_VARARGS, "Take a cancel response." }, + { + "rclpy_action_publish_feedback", rclpy_action_publish_feedback, METH_VARARGS, + "Publish a feedback message." + }, { "rclpy_action_take_feedback", rclpy_action_take_feedback, METH_VARARGS, "Take a feedback message." }, + { + "rclpy_action_publish_status", rclpy_action_publish_status, METH_VARARGS, + "Publish a status message." + }, { "rclpy_action_take_status", rclpy_action_take_status, METH_VARARGS, "Take a status message." }, + { + "rclpy_action_accept_new_goal", rclpy_action_accept_new_goal, METH_VARARGS, + "Accept a new goal using an action server." + }, + { + "rclpy_action_notify_goal_done", rclpy_action_notify_goal_done, METH_VARARGS, + "Notify and action server that a goal has reached a terminal state." + }, + { + "rclpy_action_update_goal_state", rclpy_action_update_goal_state, METH_VARARGS, + "Update a goal state." + }, + { + "rclpy_action_goal_handle_is_active", rclpy_action_goal_handle_is_active, METH_VARARGS, + "Check if a goal is active." + }, + { + "rclpy_action_server_goal_exists", rclpy_action_server_goal_exists, METH_VARARGS, + "Check if a goal being tracked by an action server." + }, + { + "rclpy_action_goal_handle_get_status", rclpy_action_goal_handle_get_status, METH_VARARGS, + "Get the status of a goal." + }, + { + "rclpy_action_process_cancel_request", rclpy_action_process_cancel_request, METH_VARARGS, + "Process a cancel request to determine what goals should be canceled." + }, + { + "rclpy_action_expire_goals", rclpy_action_expire_goals, METH_VARARGS, + "Expire goals associated with an action server." + }, {NULL, NULL, 0, NULL} /* sentinel */ }; diff --git a/rclpy/test/action/test_client.py b/rclpy/test/action/test_client.py index 9578c4b20..0317dbf07 100644 --- a/rclpy/test/action/test_client.py +++ b/rclpy/test/action/test_client.py @@ -81,10 +81,8 @@ def feedback_callback(self, feedback): def timed_spin(self, duration): start_time = time.time() - current_time = start_time - while (current_time - start_time) < duration: + while (time.time() - start_time) < duration: rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) - current_time = time.time() def test_constructor_defaults(self): # Defaults diff --git a/rclpy/test/action/test_server.py b/rclpy/test/action/test_server.py new file mode 100644 index 000000000..807c7df1b --- /dev/null +++ b/rclpy/test/action/test_server.py @@ -0,0 +1,624 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest +import uuid + +from action_msgs.msg import GoalStatus +from action_msgs.srv import CancelGoal + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor + +from test_msgs.action import Fibonacci + +from unique_identifier_msgs.msg import UUID + + +class MockActionClient(): + + def __init__(self, node): + self.reset() + self.goal_srv = node.create_client( + Fibonacci.GoalRequestService, '/fibonacci/_action/send_goal') + self.cancel_srv = node.create_client( + Fibonacci.CancelGoalService, '/fibonacci/_action/cancel_goal') + self.result_srv = node.create_client( + Fibonacci.GoalResultService, '/fibonacci/_action/get_result') + self.feedback_sub = node.create_subscription( + Fibonacci.Feedback, '/fibonacci/_action/feedback', self.feedback_callback) + self.status_sub = node.create_subscription( + Fibonacci.GoalStatusMessage, '/fibonacci/_action/status', self.status_callback) + + def reset(self): + self.feedback_msg = None + self.status_msg = None + + def feedback_callback(self, feedback_msg): + self.feedback_msg = feedback_msg + + def status_callback(self, status_msg): + self.status_msg = status_msg + + def send_goal(self, goal_msg): + return self.goal_srv.call_async(goal_msg) + + def cancel_goal(self, cancel_msg): + return self.cancel_srv.call_async(cancel_msg) + + def get_result(self, goal_uuid): + result_request = Fibonacci.GoalResultService.Request() + result_request.action_goal_id = goal_uuid + return self.result_srv.call_async(result_request) + + +class TestActionServer(unittest.TestCase): + + def setUp(self): + self.context = rclpy.context.Context() + rclpy.init(context=self.context) + self.executor = SingleThreadedExecutor(context=self.context) + self.node = rclpy.create_node('TestActionServer', context=self.context) + self.mock_action_client = MockActionClient(self.node) + + def tearDown(self): + self.node.destroy_node() + self.executor.shutdown() + rclpy.shutdown(context=self.context) + + def timed_spin(self, duration): + start_time = time.time() + while (time.time() - start_time) < duration: + rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1) + + def execute_goal_callback(self, goal_handle): + goal_handle.set_succeeded() + return Fibonacci.Result() + + def test_constructor_defaults(self): + # Defaults + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + ) + action_server.destroy() + + def test_constructor_no_defaults(self): + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + callback_group=ReentrantCallbackGroup(), + goal_callback=lambda req: GoalResponse.REJECT, + handle_accepted_callback=lambda gh: None, + cancel_callback=lambda req: CancelResponse.REJECT, + goal_service_qos_profile=rclpy.qos.qos_profile_default, + result_service_qos_profile=rclpy.qos.qos_profile_default, + cancel_service_qos_profile=rclpy.qos.qos_profile_default, + feedback_pub_qos_profile=rclpy.qos.qos_profile_default, + status_pub_qos_profile=rclpy.qos.qos_profile_default, + result_timeout=300, + ) + action_server.destroy() + + def test_get_num_entities(self): + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + ) + num_entities = action_server.get_num_entities() + self.assertEqual(num_entities.num_subscriptions, 0) + self.assertEqual(num_entities.num_guard_conditions, 0) + self.assertEqual(num_entities.num_timers, 1) + self.assertEqual(num_entities.num_clients, 0) + self.assertEqual(num_entities.num_services, 3) + action_server.destroy() + + def test_single_goal_accept(self): + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_order = 10 + + def goal_callback(goal): + nonlocal goal_uuid + nonlocal goal_order + self.assertEqual(goal.action_goal_id, goal_uuid) + self.assertEqual(goal.order, goal_order) + return GoalResponse.ACCEPT + + handle_accepted_callback_triggered = False + + def handle_accepted_callback(goal_handle): + nonlocal handle_accepted_callback_triggered + handle_accepted_callback_triggered = True + self.assertEqual(goal_handle.status, GoalStatus.STATUS_ACCEPTED) + self.assertEqual(goal_handle.goal_id, goal_uuid) + self.assertEqual(goal_handle.request.order, goal_order) + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + goal_callback=goal_callback, + handle_accepted_callback=handle_accepted_callback, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = goal_uuid + goal_msg.order = goal_order + future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, future, self.executor) + self.assertTrue(future.result().accepted) + self.assertTrue(handle_accepted_callback_triggered) + action_server.destroy() + + def test_single_goal_reject(self): + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_order = 10 + + def goal_callback(goal): + nonlocal goal_uuid + nonlocal goal_order + self.assertEqual(goal.action_goal_id, goal_uuid) + self.assertEqual(goal.order, goal_order) + return GoalResponse.REJECT + + def handle_accepted_callback(goal_handle): + # Since the goal is rejected, we don't expect this function to be called + self.assertFalse(True) + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + goal_callback=goal_callback, + handle_accepted_callback=handle_accepted_callback, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = goal_uuid + goal_msg.order = goal_order + future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, future, self.executor) + self.assertFalse(future.result().accepted) + action_server.destroy() + + def test_goal_callback_invalid_return(self): + + def goal_callback(goal): + return 'Invalid return type' + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + goal_callback=goal_callback, + handle_accepted_callback=lambda gh: None, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, future, self.executor) + # An invalid return type in the goal callback should translate to a rejected goal + self.assertFalse(future.result().accepted) + action_server.destroy() + + def test_multi_goal_accept(self): + executor = MultiThreadedExecutor(context=self.context) + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + callback_group=ReentrantCallbackGroup(), + handle_accepted_callback=lambda gh: None, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + future0 = self.mock_action_client.send_goal(goal_msg) + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + future1 = self.mock_action_client.send_goal(goal_msg) + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + future2 = self.mock_action_client.send_goal(goal_msg) + + rclpy.spin_until_future_complete(self.node, future0, executor) + rclpy.spin_until_future_complete(self.node, future1, executor) + rclpy.spin_until_future_complete(self.node, future2, executor) + + self.assertTrue(future0.result().accepted) + self.assertTrue(future1.result().accepted) + self.assertTrue(future2.result().accepted) + action_server.destroy() + + def test_duplicate_goal(self): + executor = MultiThreadedExecutor(context=self.context) + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + callback_group=ReentrantCallbackGroup(), + handle_accepted_callback=lambda gh: None, + ) + + # Send a goal with the same ID twice + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + future0 = self.mock_action_client.send_goal(goal_msg) + future1 = self.mock_action_client.send_goal(goal_msg) + + rclpy.spin_until_future_complete(self.node, future0, executor) + rclpy.spin_until_future_complete(self.node, future1, executor) + + # Exactly one of the goals should be accepted + self.assertNotEqual(future0.result().accepted, future1.result().accepted) + action_server.destroy() + + 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.set_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.Goal() + goal_msg.action_goal_id = goal_uuid + goal_msg.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) + self.assertEqual(cancel_result.goals_canceling[0].goal_id.uuid, goal_uuid.uuid) + + action_server.destroy() + executor.shutdown() + + def test_cancel_goal_reject(self): + + def execute_callback(goal_handle): + # Wait, to give the opportunity to cancel + time.sleep(3.0) + self.assertFalse(goal_handle.is_cancel_requested) + goal_handle.set_canceled() + return Fibonacci.Result() + + def cancel_callback(request): + return CancelResponse.REJECT + + 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.Goal() + goal_msg.action_goal_id = goal_uuid + goal_msg.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), 0) + + 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.set_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.Goal() + goal_msg.action_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.action_status, GoalStatus.STATUS_CANCELED) + self.assertEqual(server_goal_handle.status, GoalStatus.STATUS_CANCELED) + action_server.destroy() + + def test_execute_set_succeeded(self): + + def execute_callback(goal_handle): + self.assertEqual(goal_handle.status, GoalStatus.STATUS_EXECUTING) + result = Fibonacci.Result() + result.sequence.extend([1, 1, 2, 3, 5]) + goal_handle.set_succeeded() + return result + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_callback, + ) + + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg = Fibonacci.Goal() + goal_msg.action_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) + goal_handle = goal_future.result() + self.assertTrue(goal_handle.accepted) + + 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.action_status, GoalStatus.STATUS_SUCCEEDED) + self.assertEqual(result.sequence, [1, 1, 2, 3, 5]) + action_server.destroy() + + def test_execute_set_aborted(self): + + def execute_callback(goal_handle): + self.assertEqual(goal_handle.status, GoalStatus.STATUS_EXECUTING) + result = Fibonacci.Result() + result.sequence.extend([1, 1, 2, 3, 5]) + goal_handle.set_aborted() + return result + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_callback, + ) + + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg = Fibonacci.Goal() + goal_msg.action_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) + goal_handle = goal_future.result() + self.assertTrue(goal_handle.accepted) + + 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.action_status, GoalStatus.STATUS_ABORTED) + self.assertEqual(result.sequence, [1, 1, 2, 3, 5]) + action_server.destroy() + + def test_execute_no_terminal_state(self): + + def execute_callback(goal_handle): + # Do not set the goal handles state + result = Fibonacci.Result() + result.sequence.extend([1, 1, 2, 3, 5]) + return result + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_callback, + ) + + goal_uuid = UUID(uuid=list(uuid.uuid4().bytes)) + goal_msg = Fibonacci.Goal() + goal_msg.action_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) + goal_handle = goal_future.result() + self.assertTrue(goal_handle.accepted) + + 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() + # Goal status should default to STATUS_ABORTED + self.assertEqual(result.action_status, GoalStatus.STATUS_ABORTED) + self.assertEqual(result.sequence, [1, 1, 2, 3, 5]) + action_server.destroy() + + def test_expire_goals_none(self): + + # 1 second timeout + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + result_timeout=1, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, goal_future, self.executor) + + self.assertEqual(1, len(action_server._goal_handles)) + + # After less than one second there should still be a goal handle + self.timed_spin(0.5) + self.assertEqual(1, len(action_server._goal_handles)) + action_server.destroy() + + def test_expire_goals_one(self): + + # 1 second timeout + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + result_timeout=1, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_future = self.mock_action_client.send_goal(goal_msg) + rclpy.spin_until_future_complete(self.node, goal_future, self.executor) + + self.assertEqual(1, len(action_server._goal_handles)) + + # After two seconds the internal goal handle should be destroyed + self.timed_spin(2.1) + self.assertEqual(0, len(action_server._goal_handles)) + action_server.destroy() + + def test_expire_goals_multi(self): + # 1 second timeout + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=self.execute_goal_callback, + result_timeout=1, + ) + + # Send multiple goals + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + self.mock_action_client.send_goal(goal_msg) + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + self.mock_action_client.send_goal(goal_msg) + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + self.mock_action_client.send_goal(goal_msg) + self.timed_spin(0.5) + + self.assertEqual(3, len(action_server._goal_handles)) + + # After two seconds the internal goal handles should be destroyed + self.timed_spin(2.1) + self.assertEqual(0, len(action_server._goal_handles)) + action_server.destroy() + + def test_feedback(self): + + def execute_with_feedback(goal_handle): + feedback_msg = Fibonacci.Feedback() + feedback_msg.sequence = [1, 1, 2, 3] + goal_handle.publish_feedback(feedback_msg) + goal_handle.set_succeeded() + return Fibonacci.Result() + + action_server = ActionServer( + self.node, + Fibonacci, + 'fibonacci', + execute_callback=execute_with_feedback, + ) + + goal_msg = Fibonacci.Goal() + goal_msg.action_goal_id = UUID(uuid=list(uuid.uuid4().bytes)) + goal_future = self.mock_action_client.send_goal(goal_msg) + + rclpy.spin_until_future_complete(self.node, goal_future, self.executor) + + self.assertIsNotNone(self.mock_action_client.feedback_msg) + self.assertEqual([1, 1, 2, 3], self.mock_action_client.feedback_msg.sequence) + action_server.destroy() + + +if __name__ == '__main__': + unittest.main()