diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt
index d6c6e0764..933bc4396 100644
--- a/rclpy/CMakeLists.txt
+++ b/rclpy/CMakeLists.txt
@@ -13,6 +13,7 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rcl REQUIRED)
+find_package(rcl_action REQUIRED)
find_package(rcl_yaml_param_parser REQUIRED)
find_package(rcutils REQUIRED)
find_package(rmw REQUIRED)
@@ -71,10 +72,39 @@ function(configure_python_c_extension_library _library_name)
)
endfunction()
+add_library(rclpy_common
+ SHARED src/rclpy_common/src/common.c
+)
+target_link_libraries(rclpy_common
+ ${PythonExtra_LIBRARIES}
+)
+target_include_directories(rclpy_common
+ PUBLIC
+ src/rclpy_common/include
+ ${PythonExtra_INCLUDE_DIRS}
+)
+ament_target_dependencies(rclpy_common
+ "rmw"
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(rclpy_common PRIVATE "RCLPY_COMMON_BUILDING_DLL")
+
+install(TARGETS rclpy_common
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+)
+
add_library(
rclpy
SHARED src/rclpy/_rclpy.c
)
+target_link_libraries(rclpy
+ rclpy_common
+)
+
configure_python_c_extension_library(rclpy)
ament_target_dependencies(rclpy
"rcl"
@@ -82,6 +112,21 @@ ament_target_dependencies(rclpy
"rcutils"
)
+add_library(
+ rclpy_action
+ SHARED src/rclpy/_rclpy_action.c
+)
+target_link_libraries(rclpy_action
+ rclpy_common
+)
+
+configure_python_c_extension_library(rclpy_action)
+ament_target_dependencies(rclpy_action
+ "rcl"
+ "rcl_action"
+ "rcutils"
+)
+
# Logging support provided by rcutils
add_library(
rclpy_logging
diff --git a/rclpy/package.xml b/rclpy/package.xml
index 164b63670..59e26407f 100644
--- a/rclpy/package.xml
+++ b/rclpy/package.xml
@@ -19,7 +19,9 @@
rmw_implementation
rcl
+ rcl_action
rcl_yaml_param_parser
+ unique_identifier_msgs
ament_index_python
builtin_interfaces
diff --git a/rclpy/rclpy/action/__init__.py b/rclpy/rclpy/action/__init__.py
new file mode 100644
index 000000000..8b5bf4ce5
--- /dev/null
+++ b/rclpy/rclpy/action/__init__.py
@@ -0,0 +1,16 @@
+# 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 .client import ActionClient # noqa
+from .server import ActionServer, CancelResponse, GoalResponse # noqa
diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py
new file mode 100644
index 000000000..7ddbbe142
--- /dev/null
+++ b/rclpy/rclpy/action/client.py
@@ -0,0 +1,558 @@
+# 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 threading
+import time
+import uuid
+import weakref
+
+from action_msgs.msg import GoalStatus
+from action_msgs.srv import CancelGoal
+
+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
+
+from unique_identifier_msgs.msg import UUID
+
+
+class ClientGoalHandle():
+ """Goal handle for working with Action Clients."""
+
+ def __init__(self, action_client, goal_id, goal_response):
+ self._action_client = action_client
+ self._goal_id = goal_id
+ self._goal_response = goal_response
+ self._status = GoalStatus.STATUS_UNKNOWN
+
+ def __eq__(self, other):
+ return self._goal_id == other.goal_id
+
+ def __ne__(self, other):
+ return self._goal_id != other.goal_id
+
+ def __repr__(self):
+ return 'ClientGoalHandle '.format(
+ self.goal_id.uuid,
+ self.accepted,
+ self.status)
+
+ @property
+ def goal_id(self):
+ return self._goal_id
+
+ @property
+ def stamp(self):
+ return self._goal_response.stamp
+
+ @property
+ def accepted(self):
+ return self._goal_response.accepted
+
+ @property
+ def status(self):
+ return self._status
+
+ def cancel_goal(self):
+ """
+ Send a cancel request for the goal and wait for the response.
+
+ Do not call this method in a callback or a deadlock may occur.
+
+ :return: The cancel response.
+ """
+ return self._action_client._cancel_goal(self)
+
+ def cancel_goal_async(self):
+ """
+ Asynchronous request for the goal be canceled.
+
+ :return: a Future instance that completes when the server responds.
+ :rtype: :class:`rclpy.task.Future` instance
+ """
+ return self._action_client._cancel_goal_async(self)
+
+ def get_result(self):
+ """
+ Request the result for the goal and wait for the response.
+
+ Do not call this method in a callback or a deadlock may occur.
+
+ :return: The result response.
+ """
+ return self._action_client._get_result(self)
+
+ def get_result_async(self):
+ """
+ Asynchronously request the goal result.
+
+ :return: a Future instance that completes when the result is ready.
+ :rtype: :class:`rclpy.task.Future` instance
+ """
+ return self._action_client._get_result_async(self)
+
+
+class ActionClient(Waitable):
+ """ROS Action client."""
+
+ def __init__(
+ self,
+ node,
+ action_type,
+ action_name,
+ *,
+ callback_group=None,
+ 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_sub_qos_profile=qos_profile_default,
+ status_sub_qos_profile=qos_profile_action_status_default
+ ):
+ """
+ Constructor.
+
+ :param node: The ROS node to add the action client 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 callback_group: Callback group to add the action client to.
+ If None, then the node's default callback group is used.
+ :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_sub_qos_profile: QoS profile for the feedback subscriber.
+ :param status_sub_qos_profile: QoS profile for the status subscriber.
+ """
+ if callback_group is None:
+ callback_group = node.default_callback_group
+
+ super().__init__(callback_group)
+
+ # 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._client_handle = _rclpy_action.rclpy_action_create_client(
+ node.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_sub_qos_profile.get_c_qos_profile(),
+ status_sub_qos_profile.get_c_qos_profile()
+ )
+
+ self._is_ready = False
+
+ # key: UUID in bytes, value: weak reference to ClientGoalHandle
+ self._goal_handles = {}
+ # key: goal request sequence_number, value: Future for goal response
+ self._pending_goal_requests = {}
+ # key: goal request sequence_number, value: UUID
+ self._sequence_number_to_goal_id = {}
+ # key: cancel request sequence number, value: Future for cancel response
+ self._pending_cancel_requests = {}
+ # key: result request sequence number, value: Future for result response
+ self._pending_result_requests = {}
+ # key: UUID in bytes, value: callback function
+ self._feedback_callbacks = {}
+
+ callback_group.add_entity(self)
+ self._node.add_waitable(self)
+
+ def _generate_random_uuid(self):
+ return UUID(uuid=list(uuid.uuid4().bytes))
+
+ def _remove_pending_request(self, future, pending_requests):
+ """
+ Remove a future from the list of pending requests.
+
+ This prevents a future from receiving a request and executing its done callbacks.
+ :param future: a future returned from one of :meth:`send_goal_async`,
+ :meth:`_cancel_goal_async`, or :meth:`_get_result_async`.
+ :type future: rclpy.task.Future
+ :param pending_requests: The list of pending requests.
+ :type pending_requests: dict
+ :return: The sequence number associated with the removed future, or
+ None if the future was not found in the list.
+ """
+ for seq, req_future in list(pending_requests.items()):
+ if future == req_future:
+ try:
+ del pending_requests[seq]
+ except KeyError:
+ pass
+ else:
+ self.remove_future(future)
+ return seq
+ return None
+
+ def _remove_pending_goal_request(self, future):
+ seq = self._remove_pending_request(future, self._pending_goal_requests)
+ if seq in self._sequence_number_to_goal_id:
+ del self._sequence_number_to_goal_id[seq]
+
+ def _remove_pending_cancel_request(self, future):
+ self._remove_pending_request(future, self._pending_cancel_requests)
+
+ def _remove_pending_result_request(self, future):
+ self._remove_pending_request(future, self._pending_result_requests)
+
+ # Start Waitable API
+ def is_ready(self, wait_set):
+ """Return True if one or more entities are ready in the wait set."""
+ ready_entities = _rclpy_action.rclpy_action_wait_set_is_ready(
+ self._client_handle,
+ wait_set)
+ self._is_feedback_ready = ready_entities[0]
+ self._is_status_ready = ready_entities[1]
+ self._is_goal_response_ready = ready_entities[2]
+ self._is_cancel_response_ready = ready_entities[3]
+ self._is_result_response_ready = ready_entities[4]
+ 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_response_ready:
+ taken_data = _rclpy_action.rclpy_action_take_goal_response(
+ self._client_handle, self._action_type.GoalRequestService.Response)
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['goal'] = taken_data
+
+ if self._is_cancel_response_ready:
+ taken_data = _rclpy_action.rclpy_action_take_cancel_response(
+ self._client_handle, self._action_type.CancelGoalService.Response)
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['cancel'] = taken_data
+
+ if self._is_result_response_ready:
+ taken_data = _rclpy_action.rclpy_action_take_result_response(
+ self._client_handle, self._action_type.GoalResultService.Response)
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['result'] = taken_data
+
+ if self._is_feedback_ready:
+ taken_data = _rclpy_action.rclpy_action_take_feedback(
+ self._client_handle, self._action_type.Feedback)
+ # If take fails, then we get None
+ if taken_data is not None:
+ data['feedback'] = taken_data
+
+ if self._is_status_ready:
+ taken_data = _rclpy_action.rclpy_action_take_status(
+ self._client_handle, self._action_type.GoalStatusMessage)
+ # If take fails, then we get None
+ if taken_data is not None:
+ data['status'] = taken_data
+
+ 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:
+ sequence_number, goal_response = taken_data['goal']
+ goal_handle = ClientGoalHandle(
+ self,
+ self._sequence_number_to_goal_id[sequence_number],
+ goal_response)
+
+ if goal_handle.accepted:
+ goal_uuid = bytes(goal_handle.goal_id.uuid)
+ if goal_uuid in self._goal_handles:
+ raise RuntimeError(
+ 'Two goals were accepted with the same ID ({})'.format(goal_handle))
+ self._goal_handles[goal_uuid] = weakref.ref(goal_handle)
+
+ self._pending_goal_requests[sequence_number].set_result(goal_handle)
+
+ if 'cancel' in taken_data:
+ sequence_number, cancel_response = taken_data['cancel']
+ self._pending_cancel_requests[sequence_number].set_result(cancel_response)
+
+ if 'result' in taken_data:
+ sequence_number, result_response = taken_data['result']
+ self._pending_result_requests[sequence_number].set_result(result_response)
+
+ if 'feedback' in taken_data:
+ feedback_msg = taken_data['feedback']
+ goal_uuid = bytes(feedback_msg.action_goal_id.uuid)
+ # Call a registered callback if there is one
+ if goal_uuid in self._feedback_callbacks:
+ await await_or_execute(self._feedback_callbacks[goal_uuid], feedback_msg)
+
+ if 'status' in taken_data:
+ # Update the status of all goal handles maintained by this Action Client
+ for status_msg in taken_data['status'].status_list:
+ goal_uuid = bytes(status_msg.goal_info.goal_id.uuid)
+ status = status_msg.status
+
+ if goal_uuid in self._goal_handles:
+ goal_handle = self._goal_handles[goal_uuid]()
+ if goal_handle is not None:
+ goal_handle._status = status
+ # Remove "done" goals from the list
+ if (GoalStatus.STATUS_SUCCEEDED == status or
+ GoalStatus.STATUS_CANCELED == status or
+ GoalStatus.STATUS_ABORTED == status):
+ del self._goal_handles[goal_uuid]
+ else:
+ # Weak reference is None
+ del self._goal_handles[goal_uuid]
+
+ 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._client_handle)
+ return NumberOfEntities(*num_entities)
+
+ def add_to_wait_set(self, wait_set):
+ """Add entities to wait set."""
+ _rclpy_action.rclpy_action_wait_set_add(self._client_handle, wait_set)
+ # End Waitable API
+
+ def send_goal(self, goal, **kwargs):
+ """
+ Send a goal and wait for the result.
+
+ Do not call this method in a callback or a deadlock may occur.
+
+ See :meth:`send_goal_async` for more info about keyword arguments.
+
+ Unlike :meth:`send_goal_async`, this method returns the final result of the
+ action (not a goal handle).
+
+ :param goal: The goal request.
+ :type goal: action_type.Goal
+ :return: The result response.
+ :rtype: action_type.Result
+ """
+ event = threading.Event()
+
+ def unblock(future):
+ nonlocal event
+ event.set()
+
+ send_goal_future = self.send_goal_async(goal, kwargs)
+ send_goal_future.add_done_callback(unblock)
+
+ event.wait()
+ if send_goal_future.exception() is not None:
+ raise send_goal_future.exception()
+
+ goal_handle = send_goal_future.result()
+
+ result = self._get_result(goal_handle)
+
+ return result
+
+ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
+ """
+ Send a goal and asynchronously get the result.
+
+ The result of the returned Future is set to a ClientGoalHandle when receipt of the goal
+ is acknowledged by an action server.
+
+ :param goal: The goal request.
+ :type goal: action_type.Goal
+ :param feedback_callback: Callback function for feedback associated with the goal.
+ :type feedback_callback: function
+ :param goal_uuid: Universally unique identifier for the goal.
+ If None, then a random UUID is generated.
+ :type: unique_identifier_msgs.UUID
+ :return: a Future instance to a goal handle that completes when the goal request
+ has been accepted or rejected.
+ :rtype: :class:`rclpy.task.Future` instance
+ """
+ goal.action_goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid
+ sequence_number = _rclpy_action.rclpy_action_send_goal_request(self._client_handle, goal)
+ if sequence_number in self._pending_goal_requests:
+ raise RuntimeError(
+ 'Sequence ({}) conflicts with pending goal request'.format(sequence_number))
+
+ if feedback_callback is not None:
+ # TODO(jacobperron): Move conversion function to a general-use package
+ goal_uuid = bytes(goal.action_goal_id.uuid)
+ self._feedback_callbacks[goal_uuid] = feedback_callback
+
+ future = Future()
+ self._pending_goal_requests[sequence_number] = future
+ self._sequence_number_to_goal_id[sequence_number] = goal.action_goal_id
+ future.add_done_callback(self._remove_pending_goal_request)
+ # Add future so executor is aware
+ self.add_future(future)
+
+ return future
+
+ def _cancel_goal(self, goal_handle):
+ """
+ Send a cancel request for an active goal and wait for the response.
+
+ Do not call this method in a callback or a deadlock may occur.
+
+ :param goal_handle: Handle to the goal to cancel.
+ :type goal_handle: :class:`ClientGoalHandle`
+ :return: The cancel response.
+ """
+ event = threading.Event()
+
+ def unblock(future):
+ nonlocal event
+ event.set()
+
+ future = self._cancel_goal_async(goal_handle)
+ future.add_done_callback(unblock)
+
+ event.wait()
+ if future.exception() is not None:
+ raise future.exception()
+ return future.result()
+
+ def _cancel_goal_async(self, goal_handle):
+ """
+ Send a cancel request for an active goal and asynchronously get the result.
+
+ :param goal_handle: Handle to the goal to cancel.
+ :type goal_handle: :class:`ClientGoalHandle`
+ :return: a Future instance that completes when the cancel request has been processed.
+ :rtype: :class:`rclpy.task.Future` instance
+ """
+ if not isinstance(goal_handle, ClientGoalHandle):
+ raise TypeError(
+ 'Expected type ClientGoalHandle but received {}'.format(type(goal_handle)))
+
+ cancel_request = CancelGoal.Request()
+ cancel_request.goal_info.goal_id = goal_handle.goal_id
+ sequence_number = _rclpy_action.rclpy_action_send_cancel_request(
+ self._client_handle,
+ cancel_request)
+ if sequence_number in self._pending_cancel_requests:
+ raise RuntimeError(
+ 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number))
+
+ future = Future()
+ self._pending_cancel_requests[sequence_number] = future
+ future.add_done_callback(self._remove_pending_cancel_request)
+ # Add future so executor is aware
+ self.add_future(future)
+
+ return future
+
+ def _get_result(self, goal_handle):
+ """
+ Request the result for an active goal and wait for the response.
+
+ Do not call this method in a callback or a deadlock may occur.
+
+ :param goal_handle: Handle to the goal to get the result for.
+ :type goal_handle: :class:`ClientGoalHandle`
+ :return: The result response.
+ """
+ event = threading.Event()
+
+ def unblock(future):
+ nonlocal event
+ event.set()
+
+ future = self._get_result_async(goal_handle)
+ future.add_done_callback(unblock)
+
+ event.wait()
+ if future.exception() is not None:
+ raise future.exception()
+ return future.result()
+
+ def _get_result_async(self, goal_handle):
+ """
+ Request the result for an active goal asynchronously.
+
+ :param goal_handle: Handle to the goal to cancel.
+ :type goal_handle: :class:`ClientGoalHandle`
+ :return: a Future instance that completes when the get result 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)))
+
+ result_request = self._action_type.GoalResultService.Request()
+ result_request.action_goal_id = goal_handle.goal_id
+ sequence_number = _rclpy_action.rclpy_action_send_result_request(
+ self._client_handle,
+ result_request)
+ if sequence_number in self._pending_result_requests:
+ raise RuntimeError(
+ 'Sequence ({}) conflicts with pending result request'.format(sequence_number))
+
+ future = Future()
+ self._pending_result_requests[sequence_number] = future
+ future.add_done_callback(self._remove_pending_result_request)
+ # Add future so executor is aware
+ self.add_future(future)
+
+ return future
+
+ def server_is_ready(self):
+ """
+ Check if there is an action server ready to process requests from this client.
+
+ :return: True if an action server is ready, False otherwise.
+ """
+ return _rclpy_action.rclpy_action_server_is_available(
+ self._node.handle,
+ self._client_handle)
+
+ def wait_for_server(self, timeout_sec=None):
+ """
+ Wait for an action sever to be ready.
+
+ Returns as soon as an action server is ready for this client.
+ :param timeout_sec: Number of seconds to wait until an action server is available.
+ If None, then wait indefinitely.
+ :return: True if an action server is available, False if the timeout is exceeded.
+ """
+ # TODO(jacobperron): Remove arbitrary sleep time and return as soon as server is ready
+ # See https://github.com/ros2/rclpy/issues/58
+ sleep_time = 0.25
+ if timeout_sec is None:
+ timeout_sec = float('inf')
+ while self._node.context.ok() and not self.server_is_ready() and timeout_sec > 0.0:
+ time.sleep(sleep_time)
+ timeout_sec -= sleep_time
+
+ return self.server_is_ready()
+
+ def destroy(self):
+ """Destroy the underlying action client handle."""
+ if self._client_handle is None or self._node.handle is None:
+ return
+ _rclpy_action.rclpy_action_destroy_entity(self._client_handle, self._node.handle)
+ self._node.remove_waitable(self)
+ self._client_handle = None
+
+ def __del__(self):
+ """Destroy the underlying action client handle."""
+ self.destroy()
diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py
new file mode 100644
index 000000000..d06222866
--- /dev/null
+++ b/rclpy/rclpy/action/server.py
@@ -0,0 +1,602 @@
+# 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:
+ taken_data = _rclpy_action.rclpy_action_take_goal_request(
+ self._handle,
+ self._action_type.GoalRequestService.Request,
+ )
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['goal'] = taken_data
+
+ if self._is_cancel_request_ready:
+ with self._lock:
+ taken_data = _rclpy_action.rclpy_action_take_cancel_request(
+ self._handle,
+ self._action_type.CancelGoalService.Request,
+ )
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['cancel'] = taken_data
+
+ if self._is_result_request_ready:
+ with self._lock:
+ taken_data = _rclpy_action.rclpy_action_take_result_request(
+ self._handle,
+ self._action_type.GoalResultService.Request,
+ )
+ # If take fails, then we get (None, None)
+ if all(taken_data):
+ data['result'] = taken_data
+
+ if self._is_goal_expired:
+ with self._lock:
+ data['expired'] = _rclpy_action.rclpy_action_expire_goals(
+ self._handle,
+ len(self._goal_handles),
+ )
+
+ 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/rclpy/executors.py b/rclpy/rclpy/executors.py
index b0ac69e6a..fd8115821 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -149,6 +149,7 @@ def create_task(self, callback, *args, **kwargs):
task = Task(callback, args, kwargs, executor=self)
with self._tasks_lock:
self._tasks.append((task, None, None))
+ _rclpy.rclpy_trigger_guard_condition(self._guard_condition)
# Task inherits from Future
return task
@@ -287,6 +288,11 @@ def _take_guard_condition(self, gc):
async def _execute_guard_condition(self, gc, _):
await await_or_execute(gc.callback)
+ async def _execute_waitable(self, waitable, data):
+ for future in waitable._futures:
+ future._set_executor(self)
+ await waitable.execute(data)
+
def _make_handler(self, entity, node, take_from_wait_list, call_coroutine):
"""
Make a handler that performs work on an entity.
@@ -358,20 +364,22 @@ def _wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
if nodes is None:
nodes = self.get_nodes()
- # Yield tasks in-progress before waiting for new work
- tasks = None
- with self._tasks_lock:
- tasks = list(self._tasks)
- if tasks:
- for task, entity, node in reversed(tasks):
- if not task.executing() and not task.done() and (node is None or node in nodes):
- yield task, entity, node
- with self._tasks_lock:
- # Get rid of any tasks that are done
- self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
-
yielded_work = False
while not yielded_work and not self._is_shutdown:
+ # Yield tasks in-progress before waiting for new work
+ tasks = None
+ with self._tasks_lock:
+ tasks = list(self._tasks)
+ if tasks:
+ for task, entity, node in reversed(tasks):
+ if (not task.executing() and not task.done() and
+ (node is None or node in nodes)):
+ yielded_work = True
+ yield task, entity, node
+ with self._tasks_lock:
+ # Get rid of any tasks that are done
+ self._tasks = list(filter(lambda t_e_n: not t_e_n[0].done(), self._tasks))
+
# Gather entities that can be waited on
subscriptions = []
guards = []
@@ -453,9 +461,10 @@ def _wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
# Check waitables before wait set is destroyed
for node in nodes:
for wt in node.waitables:
- if wt.is_ready(wait_set):
+ # Only check waitables that were added to the wait set
+ if wt in waitables and wt.is_ready(wait_set):
handler = self._make_handler(
- wt, node, lambda e: e.take_data(), lambda e, a: e.execute(a))
+ wt, node, lambda e: e.take_data(), self._execute_waitable)
yielded_work = True
yield handler, wt, node
diff --git a/rclpy/rclpy/impl/implementation_singleton.py b/rclpy/rclpy/impl/implementation_singleton.py
index 1f984bd66..70a04132b 100644
--- a/rclpy/rclpy/impl/implementation_singleton.py
+++ b/rclpy/rclpy/impl/implementation_singleton.py
@@ -29,4 +29,5 @@
from rclpy.impl import _import
rclpy_implementation = _import('._rclpy')
+rclpy_action_implementation = _import('._rclpy_action')
rclpy_logging_implementation = _import('._rclpy_logging')
diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py
index a50bcd122..01bd2f4f2 100644
--- a/rclpy/rclpy/node.py
+++ b/rclpy/rclpy/node.py
@@ -20,7 +20,6 @@
from rclpy.clock import ROSClock
from rclpy.constants import S_TO_NS
from rclpy.exceptions import NotInitializedException
-from rclpy.exceptions import NoTypeSupportImportedException
from rclpy.expand_topic_name import expand_topic_name
from rclpy.guard_condition import GuardCondition
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
@@ -34,6 +33,7 @@
from rclpy.subscription import Subscription
from rclpy.time_source import TimeSource
from rclpy.timer import WallTimer
+from rclpy.type_support import check_for_type_support
from rclpy.utilities import get_default_context
from rclpy.validate_full_topic_name import validate_full_topic_name
from rclpy.validate_namespace import validate_namespace
@@ -43,22 +43,6 @@
HIDDEN_NODE_PREFIX = '_'
-def check_for_type_support(msg_type):
- try:
- ts = msg_type.__class__._TYPE_SUPPORT
- except AttributeError as e:
- e.args = (
- e.args[0] +
- ' This might be a ROS 1 message type but it should be a ROS 2 message type.'
- ' Make sure to source your ROS 2 workspace after your ROS 1 workspace.',
- *e.args[1:])
- raise
- if ts is None:
- msg_type.__class__.__import_type_support__()
- if msg_type.__class__._TYPE_SUPPORT is None:
- raise NoTypeSupportImportedException()
-
-
class Node:
def __init__(
@@ -128,17 +112,24 @@ def executor(self):
def executor(self, new_executor):
"""Set or change the executor the node belongs to."""
current_executor = self.executor
+ if current_executor == new_executor:
+ return
if current_executor is not None:
current_executor.remove_node(self)
if new_executor is None:
self.__executor_weakref = None
- elif new_executor.add_node(self):
+ else:
+ new_executor.add_node(self)
self.__executor_weakref = weakref.ref(new_executor)
@property
def context(self):
return self._context
+ @property
+ def default_callback_group(self):
+ return self._default_callback_group
+
@property
def handle(self):
return self._handle
@@ -254,7 +245,7 @@ def create_subscription(
self, msg_type, topic, callback, *, qos_profile=qos_profile_default,
callback_group=None, raw=False):
if callback_group is None:
- callback_group = self._default_callback_group
+ callback_group = self.default_callback_group
# this line imports the typesupport for the message module if not already done
check_for_type_support(msg_type)
failed = False
@@ -277,7 +268,7 @@ def create_client(
self, srv_type, srv_name, *, qos_profile=qos_profile_services_default,
callback_group=None):
if callback_group is None:
- callback_group = self._default_callback_group
+ callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
@@ -302,7 +293,7 @@ def create_service(
self, srv_type, srv_name, callback, *, qos_profile=qos_profile_services_default,
callback_group=None):
if callback_group is None:
- callback_group = self._default_callback_group
+ callback_group = self.default_callback_group
check_for_type_support(srv_type)
failed = False
try:
@@ -325,7 +316,7 @@ def create_service(
def create_timer(self, timer_period_sec, callback, callback_group=None):
timer_period_nsec = int(float(timer_period_sec) * S_TO_NS)
if callback_group is None:
- callback_group = self._default_callback_group
+ callback_group = self.default_callback_group
timer = WallTimer(callback, callback_group, timer_period_nsec, context=self.context)
self.timers.append(timer)
@@ -334,7 +325,7 @@ def create_timer(self, timer_period_sec, callback, callback_group=None):
def create_guard_condition(self, callback, callback_group=None):
if callback_group is None:
- callback_group = self._default_callback_group
+ callback_group = self.default_callback_group
guard = GuardCondition(callback, callback_group, context=self.context)
self.guards.append(guard)
diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py
index b984e2a8a..07600175e 100644
--- a/rclpy/rclpy/qos.py
+++ b/rclpy/rclpy/qos.py
@@ -14,6 +14,7 @@
from enum import IntEnum
+from rclpy.impl.implementation_singleton import rclpy_action_implementation as _rclpy_action
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
@@ -178,3 +179,5 @@ class QoSDurabilityPolicy(IntEnum):
'qos_profile_parameters')
qos_profile_parameter_events = _rclpy.rclpy_get_rmw_qos_profile(
'qos_profile_parameter_events')
+qos_profile_action_status_default = _rclpy_action.rclpy_action_get_rmw_qos_profile(
+ 'rcl_action_qos_profile_status_default')
diff --git a/rclpy/rclpy/type_support.py b/rclpy/rclpy/type_support.py
new file mode 100644
index 000000000..ab576579b
--- /dev/null
+++ b/rclpy/rclpy/type_support.py
@@ -0,0 +1,31 @@
+# Copyright 2016 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 rclpy.exceptions import NoTypeSupportImportedException
+
+
+def check_for_type_support(msg_type):
+ try:
+ ts = msg_type.__class__._TYPE_SUPPORT
+ except AttributeError as e:
+ e.args = (
+ e.args[0] +
+ ' This might be a ROS 1 message type but it should be a ROS 2 message type.'
+ ' Make sure to source your ROS 2 workspace after your ROS 1 workspace.',
+ *e.args[1:])
+ raise
+ if ts is None:
+ msg_type.__class__.__import_type_support__()
+ if msg_type.__class__._TYPE_SUPPORT is None:
+ raise NoTypeSupportImportedException()
diff --git a/rclpy/rclpy/waitable.py b/rclpy/rclpy/waitable.py
index 5ce7caf03..030da2461 100644
--- a/rclpy/rclpy/waitable.py
+++ b/rclpy/rclpy/waitable.py
@@ -51,11 +51,19 @@ class Waitable:
"""
def __init__(self, callback_group):
- # A callback group to control whein this entity can execute (used by Executor)
+ # A callback group to control when this entity can execute (used by Executor)
self.callback_group = callback_group
self.callback_group.add_entity(self)
# Flag set by executor when a handler has been created but not executed (used by Executor)
self._executor_event = False
+ # List of Futures that have callbacks needing execution
+ self._futures = []
+
+ def add_future(self, future):
+ self._futures.append(future)
+
+ def remove_future(self, future):
+ self._futures.remove(future)
def is_ready(self, wait_set):
"""Return True if entities are ready in the wait set."""
diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c
index 4d444d599..11320b841 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -38,6 +38,8 @@
#include
+#include "rclpy_common/common.h"
+
static rcl_guard_condition_t * g_sigint_gc_handle;
#ifdef _WIN32
@@ -63,22 +65,6 @@ static void catch_function(int signo)
}
}
-typedef void * create_ros_message_signature (void);
-typedef void destroy_ros_message_signature (void *);
-typedef bool convert_from_py_signature (PyObject *, void *);
-typedef PyObject * convert_to_py_signature (void *);
-
-static void * get_capsule_pointer(PyObject * pymetaclass, const char * attr)
-{
- PyObject * pyattr = PyObject_GetAttrString(pymetaclass, attr);
- if (!pyattr) {
- return NULL;
- }
- void * ptr = PyCapsule_GetPointer(pyattr, NULL);
- Py_DECREF(pyattr);
- return ptr;
-}
-
void
_rclpy_context_capsule_destructor(PyObject * capsule)
{
@@ -1232,36 +1218,9 @@ rclpy_publish(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}
- PyObject * pymsg_type = PyObject_GetAttrString(pymsg, "__class__");
-
- PyObject * pymetaclass = PyObject_GetAttrString(pymsg_type, "__class__");
- Py_DECREF(pymsg_type);
-
- 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");
-
- 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 mustn't have been imported");
-
- Py_DECREF(pymetaclass);
-
- void * raw_ros_message = create_ros_message();
+ 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 PyErr_NoMemory();
- }
-
- if (!convert_from_py(pymsg, raw_ros_message)) {
- // the function has set the Python error
- destroy_ros_message(raw_ros_message);
return NULL;
}
@@ -1770,6 +1729,7 @@ rclpy_create_client(PyObject * Py_UNUSED(self), PyObject * args)
PyObject * pymetaclass = PyObject_GetAttrString(pysrv_type, "__class__");
PyObject * pyts = PyObject_GetAttrString(pymetaclass, "_TYPE_SUPPORT");
+ Py_DECREF(pymetaclass);
rosidl_service_type_support_t * ts =
(rosidl_service_type_support_t *)PyCapsule_GetPointer(pyts, NULL);
@@ -1834,37 +1794,9 @@ rclpy_send_request(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}
- PyObject * pyrequest_type = PyObject_GetAttrString(pyrequest, "__class__");
- assert(pyrequest_type != NULL);
-
- PyObject * pymetaclass = PyObject_GetAttrString(pyrequest_type, "__class__");
- assert(pymetaclass != 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");
-
- 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 mustn't 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;
}
@@ -1998,39 +1930,10 @@ rclpy_send_response(PyObject * Py_UNUSED(self), PyObject * args)
if (!header) {
return NULL;
}
- PyObject * pyresponse_type = PyObject_GetAttrString(pyresponse, "__class__");
- assert(pyresponse_type != NULL);
-
- PyObject * pymetaclass = PyObject_GetAttrString(pyresponse_type, "__class__");
- assert(pymetaclass != NULL);
-
- Py_DECREF(pyresponse_type);
-
- 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");
-
- 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 mustn't have been imported");
-
- Py_DECREF(pymetaclass);
- void * raw_ros_response = create_ros_message();
+ 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 PyErr_NoMemory();
- }
-
- if (!convert_from_py(pyresponse, raw_ros_response)) {
- // the function has set the Python error
- destroy_ros_message(raw_ros_response);
return NULL;
}
@@ -2676,22 +2579,10 @@ rclpy_take(PyObject * Py_UNUSED(self), PyObject * args)
return rclpy_take_raw(subscription);
}
- PyObject * pymetaclass = PyObject_GetAttrString(pymsg_type, "__class__");
-
- 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_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) {
- Py_DECREF(pymetaclass);
- return PyErr_NoMemory();
+ return NULL;
}
rcl_ret_t ret = rcl_take(subscription, taken_msg, NULL);
@@ -2701,15 +2592,11 @@ rclpy_take(PyObject * Py_UNUSED(self), PyObject * args)
"Failed to take from a subscription: %s", rcl_get_error_string().str);
rcl_reset_error();
destroy_ros_message(taken_msg);
- Py_DECREF(pymetaclass);
return NULL;
}
if (ret != RCL_RET_SUBSCRIPTION_TAKE_FAILED) {
- convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY");
- Py_DECREF(pymetaclass);
-
- PyObject * pytaken_msg = convert_to_py(taken_msg);
+ PyObject * pytaken_msg = rclpy_convert_to_py(taken_msg, pymsg_type);
destroy_ros_message(taken_msg);
if (!pytaken_msg) {
// the function has set the Python error
@@ -2721,7 +2608,6 @@ rclpy_take(PyObject * Py_UNUSED(self), PyObject * args)
// if take failed, just do nothing
destroy_ros_message(taken_msg);
- Py_DECREF(pymetaclass);
Py_RETURN_NONE;
}
@@ -2751,23 +2637,10 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}
- PyObject * pymetaclass = PyObject_GetAttrString(pyrequest_type, "__class__");
-
- 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_request = create_ros_message();
-
+ destroy_ros_message_signature * destroy_ros_message = NULL;
+ void * taken_request = rclpy_create_from_py(pyrequest_type, &destroy_ros_message);
if (!taken_request) {
- Py_DECREF(pymetaclass);
- return PyErr_NoMemory();
+ return NULL;
}
rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t));
@@ -2779,19 +2652,13 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args)
rcl_reset_error();
destroy_ros_message(taken_request);
PyMem_Free(header);
- Py_DECREF(pymetaclass);
return NULL;
}
if (ret != RCL_RET_SERVICE_TAKE_FAILED) {
- convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY");
- Py_DECREF(pymetaclass);
-
- PyObject * pytaken_request = convert_to_py(taken_request);
+ PyObject * pytaken_request = rclpy_convert_to_py(taken_request, pyrequest_type);
destroy_ros_message(taken_request);
if (!pytaken_request) {
- // the function has set the Python error
- PyMem_Free(header);
return NULL;
}
@@ -2804,7 +2671,6 @@ rclpy_take_request(PyObject * Py_UNUSED(self), PyObject * args)
// if take_request failed, just do nothing
PyMem_Free(header);
destroy_ros_message(taken_request);
- Py_DECREF(pymetaclass);
Py_RETURN_NONE;
}
@@ -2831,24 +2697,12 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args)
return NULL;
}
- PyObject * pymetaclass = PyObject_GetAttrString(pyresponse_type, "__class__");
-
- 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();
+ destroy_ros_message_signature * destroy_ros_message = NULL;
+ void * taken_response = rclpy_create_from_py(pyresponse_type, &destroy_ros_message);
if (!taken_response) {
- // the function has set the Python error
- Py_DECREF(pymetaclass);
return NULL;
}
+
rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t));
rcl_ret_t ret = rcl_take_response(client, header, taken_response);
int64_t sequence = header->sequence_number;
@@ -2861,10 +2715,7 @@ rclpy_take_response(PyObject * Py_UNUSED(self), PyObject * args)
}
if (ret != RCL_RET_CLIENT_TAKE_FAILED) {
- 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);
+ PyObject * pytaken_response = rclpy_convert_to_py(taken_response, pyresponse_type);
destroy_ros_message(taken_response);
if (!pytaken_response) {
// the function has set the Python error
@@ -2886,7 +2737,6 @@ rclpy_take_response(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);
return pytuple;
}
@@ -3420,34 +3270,6 @@ rclpy_convert_from_py_qos_policy(PyObject * Py_UNUSED(self), PyObject * args)
return pyqos_profile;
}
-/// Convert a C rmw_qos_profile_t into a Python QoSProfile object
-/**
- * \param[in] void pointer to a rmw_qos_profile_t structure
- * \return QoSProfile object
- */
-static PyObject *
-rclpy_convert_to_py_qos_policy(void * profile)
-{
- PyObject * pyqos_module = PyImport_ImportModule("rclpy.qos");
- PyObject * pyqos_policy_class = PyObject_GetAttrString(pyqos_module, "QoSProfile");
- PyObject * pyqos_profile = NULL;
- rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)profile;
- pyqos_profile = PyObject_CallObject(pyqos_policy_class, NULL);
- assert(pyqos_profile != NULL);
-
- PyObject_SetAttrString(pyqos_profile, "depth", PyLong_FromSize_t(qos_profile->depth));
- PyObject_SetAttrString(pyqos_profile, "history", PyLong_FromUnsignedLong(qos_profile->history));
- PyObject_SetAttrString(pyqos_profile, "reliability",
- PyLong_FromUnsignedLong(qos_profile->reliability));
- PyObject_SetAttrString(pyqos_profile, "durability",
- PyLong_FromUnsignedLong(qos_profile->durability));
- PyObject_SetAttrString(pyqos_profile, "avoid_ros_namespace_conventions",
- PyBool_FromLong(qos_profile->avoid_ros_namespace_conventions));
-
- assert(pyqos_profile != NULL);
- return pyqos_profile;
-}
-
/// Fetch a predefined qos_profile from rmw and convert it to a Python QoSProfile Object
/**
* Raises RuntimeError if there is an rcl error
diff --git a/rclpy/src/rclpy/_rclpy_action.c b/rclpy/src/rclpy/_rclpy_action.c
new file mode 100644
index 000000000..7119c273a
--- /dev/null
+++ b/rclpy/src/rclpy/_rclpy_action.c
@@ -0,0 +1,1845 @@
+// 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.
+
+#include
+
+#include
+#include
+
+#include "rclpy_common/common.h"
+
+/// Destroy an rcl_action entity.
+/**
+ * Raises RuntimeError on failure.
+ *
+ * \param[in] pyentity Capsule pointing to the entity to destroy.
+ * \param[in] pynode Capsule pointing to the node the action client was added to.
+ */
+static PyObject *
+rclpy_action_destroy_entity(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pyentity;
+ PyObject * pynode;
+
+ if (!PyArg_ParseTuple(args, "OO", &pyentity, &pynode)) {
+ return NULL;
+ }
+
+ rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t");
+ if (!node) {
+ return NULL;
+ }
+
+ rcl_ret_t ret;
+ if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) {
+ rcl_action_client_t * action_client =
+ (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t");
+ ret = rcl_action_client_fini(action_client, node);
+ PyMem_Free(action_client);
+ } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) {
+ rcl_action_server_t * action_server =
+ (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t");
+ ret = rcl_action_server_fini(action_server, node);
+ PyMem_Free(action_server);
+ } else {
+ ret = RCL_RET_ERROR; // to avoid a linter warning
+ const char * entity_name = PyCapsule_GetName(pyentity);
+ if (!entity_name) {
+ return NULL;
+ }
+ return PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", entity_name);
+ }
+
+ if (ret != RCL_RET_OK) {
+ PyErr_Format(PyExc_RuntimeError,
+ "Failed to fini '%s': %s", PyCapsule_GetName(pyentity), rcl_get_error_string().str);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ if (PyCapsule_SetPointer(pyentity, Py_None)) {
+ // exception set by PyCapsule_SetPointer
+ return NULL;
+ }
+
+ 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.
+ *
+ * This function takes a string defining a rmw_qos_profile_t and returns the
+ * corresponding Python QoSProfile object.
+ * \param[in] rmw_profile String with the name of the profile to load.
+ * \return QoSProfile object.
+ */
+static PyObject *
+rclpy_action_get_rmw_qos_profile(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ const char * rmw_profile;
+ if (!PyArg_ParseTuple(args, "s", &rmw_profile)) {
+ return NULL;
+ }
+
+ PyObject * pyqos_profile = NULL;
+ if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) {
+ pyqos_profile = rclpy_convert_to_py_qos_policy((void *)&rcl_action_qos_profile_status_default);
+ } else {
+ return PyErr_Format(PyExc_RuntimeError,
+ "Requested unknown rmw_qos_profile: '%s'", rmw_profile);
+ }
+ return pyqos_profile;
+}
+
+/// Add an action entitiy to a wait set.
+/**
+ * Raises RuntimeError on failure.
+ * \param[in] pyentity Capsule pointer to an action entity
+ * (rcl_action_client_t or rcl_action_server_t).
+ * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t.
+ */
+static PyObject *
+rclpy_action_wait_set_add(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pyentity;
+ PyObject * pywait_set;
+
+ if (!PyArg_ParseTuple(args, "OO", &pyentity, &pywait_set)) {
+ return NULL;
+ }
+
+ rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
+ if (!wait_set) {
+ return NULL;
+ }
+
+ rcl_ret_t ret;
+ if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) {
+ rcl_action_client_t * action_client =
+ (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t");
+
+ ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL);
+ } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) {
+ rcl_action_server_t * action_server =
+ (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t");
+ ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL);
+ } else {
+ ret = RCL_RET_ERROR; // to avoid linter warning
+ const char * entity_name = PyCapsule_GetName(pyentity);
+ if (!entity_name) {
+ return NULL;
+ }
+ return PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", entity_name);
+ }
+
+ if (RCL_RET_OK != ret) {
+ PyErr_Format(PyExc_RuntimeError, "Failed to add '%s' to wait set: %s",
+ PyCapsule_GetName(pyentity), rcl_get_error_string().str);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ Py_RETURN_NONE;
+}
+
+/// Get the number of wait set entities that make up an action entity.
+/**
+ * \param[in] pyentity Capsule pointer to an action entity
+ * (rcl_action_client_t or rcl_action_server_t).
+ * \return Tuple containing the number of wait set entities:
+ * (num_subscriptions,
+ * num_guard_conditions,
+ * num_timers,
+ * num_clients,
+ * num_services)
+ */
+static PyObject *
+rclpy_action_wait_set_get_num_entities(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pyentity;
+
+ if (!PyArg_ParseTuple(args, "O", &pyentity)) {
+ return NULL;
+ }
+
+ size_t num_subscriptions = 0u;
+ size_t num_guard_conditions = 0u;
+ size_t num_timers = 0u;
+ size_t num_clients = 0u;
+ size_t num_services = 0u;
+
+ rcl_ret_t ret;
+ if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) {
+ rcl_action_client_t * action_client =
+ (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t");
+
+ ret = rcl_action_client_wait_set_get_num_entities(
+ action_client,
+ &num_subscriptions,
+ &num_guard_conditions,
+ &num_timers,
+ &num_clients,
+ &num_services);
+ } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) {
+ rcl_action_server_t * action_server =
+ (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t");
+
+ ret = rcl_action_server_wait_set_get_num_entities(
+ action_server,
+ &num_subscriptions,
+ &num_guard_conditions,
+ &num_timers,
+ &num_clients,
+ &num_services);
+ } else {
+ ret = RCL_RET_ERROR; // to avoid linter warning
+ const char * entity_name = PyCapsule_GetName(pyentity);
+ if (!entity_name) {
+ return NULL;
+ }
+ return PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", entity_name);
+ }
+
+ if (RCL_RET_OK != ret) {
+ PyErr_Format(PyExc_RuntimeError,
+ "Failed to get number of entities for '%s': %s",
+ PyCapsule_GetName(pyentity),
+ rcl_get_error_string().str);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ PyObject * result_tuple = PyTuple_New(5);
+ if (!result_tuple) {
+ return NULL;
+ }
+
+ // PyTuple_SetItem() returns 0 on success
+ int set_result = 0;
+ set_result += PyTuple_SetItem(result_tuple, 0, PyLong_FromSize_t(num_subscriptions));
+ set_result += PyTuple_SetItem(result_tuple, 1, PyLong_FromSize_t(num_guard_conditions));
+ set_result += PyTuple_SetItem(result_tuple, 2, PyLong_FromSize_t(num_timers));
+ set_result += PyTuple_SetItem(result_tuple, 3, PyLong_FromSize_t(num_clients));
+ set_result += PyTuple_SetItem(result_tuple, 4, PyLong_FromSize_t(num_services));
+
+ if (0 != set_result) {
+ Py_DECREF(result_tuple);
+ return NULL;
+ }
+ return result_tuple;
+}
+
+/// Check if an action entity has any ready wait set entities.
+/**
+ * This must be called after waiting on the wait set.
+ * Raises RuntimeError on failure.
+ *
+ * \param[in] entity Capsule pointing to the action entity
+ * (rcl_action_client_t or rcl_action_server_t).
+ * \param[in] pywait_set Capsule pointing to the wait set structure.
+ * \return A tuple of Bool representing the ready sub-entities.
+ * For a rcl_action_client_t:
+ * (is_feedback_ready,
+ * is_status_ready,
+ * is_goal_response_ready,
+ * is_cancel_response_ready,
+ * is_result_response_ready)
+ *
+ * For a rcl_action_server_t:
+ * (is_goal_request_ready,
+ * is_cancel_request_ready,
+ * is_result_request_ready,
+ * is_goal_expired)
+ */
+static PyObject *
+rclpy_action_wait_set_is_ready(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pyentity;
+ PyObject * pywait_set;
+
+ if (!PyArg_ParseTuple(args, "OO", &pyentity, &pywait_set)) {
+ return NULL;
+ }
+
+ rcl_wait_set_t * wait_set = (rcl_wait_set_t *)PyCapsule_GetPointer(pywait_set, "rcl_wait_set_t");
+ if (!wait_set) {
+ return NULL;
+ }
+
+ if (PyCapsule_IsValid(pyentity, "rcl_action_client_t")) {
+ rcl_action_client_t * action_client =
+ (rcl_action_client_t *)PyCapsule_GetPointer(pyentity, "rcl_action_client_t");
+ bool is_feedback_ready = false;
+ bool is_status_ready = false;
+ bool is_goal_response_ready = false;
+ bool is_cancel_response_ready = false;
+ bool is_result_response_ready = false;
+ rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready(
+ wait_set,
+ action_client,
+ &is_feedback_ready,
+ &is_status_ready,
+ &is_goal_response_ready,
+ &is_cancel_response_ready,
+ &is_result_response_ready);
+ if (RCL_RET_OK != ret) {
+ PyErr_Format(PyExc_RuntimeError,
+ "Failed to get number of ready entities for action client: %s",
+ rcl_get_error_string().str);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ PyObject * result_tuple = PyTuple_New(5);
+ if (!result_tuple) {
+ return NULL;
+ }
+
+ // PyTuple_SetItem() returns 0 on success
+ int set_result = 0;
+ set_result += PyTuple_SetItem(result_tuple, 0, PyBool_FromLong(is_feedback_ready));
+ set_result += PyTuple_SetItem(result_tuple, 1, PyBool_FromLong(is_status_ready));
+ set_result += PyTuple_SetItem(result_tuple, 2, PyBool_FromLong(is_goal_response_ready));
+ set_result += PyTuple_SetItem(result_tuple, 3, PyBool_FromLong(is_cancel_response_ready));
+ set_result += PyTuple_SetItem(result_tuple, 4, PyBool_FromLong(is_result_response_ready));
+ if (0 != set_result) {
+ Py_DECREF(result_tuple);
+ return NULL;
+ }
+ return result_tuple;
+ } else if (PyCapsule_IsValid(pyentity, "rcl_action_server_t")) {
+ rcl_action_server_t * action_server =
+ (rcl_action_server_t *)PyCapsule_GetPointer(pyentity, "rcl_action_server_t");
+ bool is_goal_request_ready = false;
+ bool is_cancel_request_ready = false;
+ bool is_result_request_ready = false;
+ bool is_goal_expired = false;
+ rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready(
+ wait_set,
+ action_server,
+ &is_goal_request_ready,
+ &is_cancel_request_ready,
+ &is_result_request_ready,
+ &is_goal_expired);
+ if (RCL_RET_OK != ret) {
+ PyErr_Format(PyExc_RuntimeError,
+ "Failed to get number of ready entities for action server: %s",
+ rcl_get_error_string().str);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ PyObject * result_tuple = PyTuple_New(4);
+ if (!result_tuple) {
+ return NULL;
+ }
+
+ // PyTuple_SetItem() returns 0 on success
+ int set_result = 0;
+ set_result += PyTuple_SetItem(result_tuple, 0, PyBool_FromLong(is_goal_request_ready));
+ set_result += PyTuple_SetItem(result_tuple, 1, PyBool_FromLong(is_cancel_request_ready));
+ set_result += PyTuple_SetItem(result_tuple, 2, PyBool_FromLong(is_result_request_ready));
+ set_result += PyTuple_SetItem(result_tuple, 3, PyBool_FromLong(is_goal_expired));
+ if (0 != set_result) {
+ Py_DECREF(result_tuple);
+ return NULL;
+ }
+ return result_tuple;
+ } else {
+ const char * entity_name = PyCapsule_GetName(pyentity);
+ if (!entity_name) {
+ return NULL;
+ }
+ return PyErr_Format(PyExc_RuntimeError, "'%s' is not a known entity", entity_name);
+ }
+}
+
+#define OPTIONS_COPY_QOS_PROFILE(Options, Profile) \
+ { \
+ void * p = PyCapsule_GetPointer(py ## Profile, "rmw_qos_profile_t"); \
+ if (!p) { \
+ return NULL; \
+ } \
+ rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)p; \
+ Options.Profile = * qos_profile; \
+ PyMem_Free(p); \
+ if (PyCapsule_SetPointer(py ## Profile, Py_None)) { \
+ /* exception set by PyCapsule_SetPointer */ \
+ return NULL; \
+ } \
+ }
+
+/// Create an action client.
+/**
+ * This function will create an action client for the given action name.
+ * This client 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_client_t structure
+ * is returned.
+ *
+ * Raises ValueError if action name is invalid
+ * Raises RuntimeError if the action client could not be created.
+ *
+ * \remark Call rclpy_action_destroy_entity() to destroy an action client.
+ * \param[in] pynode Capsule pointing to the node to add the action client to.
+ * \param[in] pyaction_type Action module associated with the action client.
+ * \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_client_t', or
+ * \return NULL on failure.
+ */
+static PyObject *
+rclpy_action_create_client(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pynode;
+ 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;
+
+ int parse_tuple_result = PyArg_ParseTuple(
+ args,
+ "OOOOOOOO",
+ &pynode,
+ &pyaction_type,
+ &pyaction_name,
+ &pygoal_service_qos,
+ &pyresult_service_qos,
+ &pycancel_service_qos,
+ &pyfeedback_topic_qos,
+ &pystatus_topic_qos);
+
+ 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;
+ }
+
+ 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_client_options_t action_client_ops = rcl_action_client_get_default_options();
+
+ OPTIONS_COPY_QOS_PROFILE(action_client_ops, goal_service_qos);
+ OPTIONS_COPY_QOS_PROFILE(action_client_ops, result_service_qos);
+ OPTIONS_COPY_QOS_PROFILE(action_client_ops, cancel_service_qos);
+ OPTIONS_COPY_QOS_PROFILE(action_client_ops, feedback_topic_qos);
+ OPTIONS_COPY_QOS_PROFILE(action_client_ops, status_topic_qos);
+
+ rcl_action_client_t * action_client =
+ (rcl_action_client_t *)PyMem_Malloc(sizeof(rcl_action_client_t));
+ if (!action_client) {
+ return PyErr_NoMemory();
+ }
+ *action_client = rcl_action_get_zero_initialized_client();
+ rcl_ret_t ret = rcl_action_client_init(
+ action_client,
+ node,
+ ts,
+ action_name,
+ &action_client_ops);
+ if (ret != RCL_RET_OK) {
+ if (ret == RCL_RET_ACTION_NAME_INVALID) {
+ PyErr_Format(PyExc_ValueError,
+ "Failed to create action client due to invalid topic name '%s': %s",
+ action_name, rcl_get_error_string().str);
+ } else {
+ PyErr_Format(PyExc_RuntimeError,
+ "Failed to create action client: %s", rcl_get_error_string().str);
+ }
+ PyMem_Free(action_client);
+ rcl_reset_error();
+ return NULL;
+ }
+
+ 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.
+ *
+ * \param[in] pynode Capsule pointing to the node to associated with the action client.
+ * \param[in] pyaction_client The action client to use when checking for an available server.
+ * \return True if an action server is available, False otherwise.
+ */
+static PyObject *
+rclpy_action_server_is_available(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ PyObject * pynode;
+ PyObject * pyaction_client;
+
+ if (!PyArg_ParseTuple(args, "OO", &pynode, &pyaction_client)) {
+ return NULL;
+ }
+
+ rcl_node_t * node = (rcl_node_t *)PyCapsule_GetPointer(pynode, "rcl_node_t");
+ if (!node) {
+ return NULL;
+ }
+ rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer(
+ pyaction_client, "rcl_action_client_t");
+ if (!action_client) {
+ return NULL;
+ }
+
+ bool is_available = false;
+ rcl_ret_t ret = rcl_action_server_is_available(node, action_client, &is_available);
+ if (RCL_RET_OK != ret) {
+ return PyErr_Format(PyExc_RuntimeError,
+ "Failed to check if action server is available: %s", rcl_get_error_string().str);
+ }
+
+ if (is_available) {
+ Py_RETURN_TRUE;
+ }
+ Py_RETURN_FALSE;
+}
+
+#define SEND_SERVICE_REQUEST(Type) \
+ PyObject * pyaction_client; \
+ PyObject * pyrequest; \
+ if (!PyArg_ParseTuple(args, "OO", & pyaction_client, & pyrequest)) { \
+ return NULL; \
+ } \
+ rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer( \
+ pyaction_client, "rcl_action_client_t"); \
+ if (!action_client) { \
+ return NULL; \
+ } \
+ 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 NULL; \
+ } \
+ int64_t sequence_number; \
+ rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \
+ action_client, raw_ros_request, & sequence_number); \
+ destroy_ros_message(raw_ros_request); \
+ if (ret != RCL_RET_OK) { \
+ PyErr_Format(PyExc_RuntimeError, \
+ "Failed to send " #Type " request: %s", rcl_get_error_string().str); \
+ rcl_reset_error(); \
+ return NULL; \
+ } \
+ 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 * 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( \
+ pyaction_client, "rcl_action_client_t"); \
+ if (!action_client) { \
+ 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) { \
+ 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 ## _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) { \
+ destroy_ros_message(taken_msg); \
+ 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); \
+ 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); \
+ return NULL; \
+ } \
+ PyObject * pysequence = PyLong_FromLongLong(sequence); \
+ if (!pysequence) { \
+ Py_DECREF(pytaken_msg); \
+ Py_DECREF(pytuple); \
+ return NULL; \
+ } \
+ PyTuple_SET_ITEM(pytuple, 0, pysequence); \
+ 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.
+ * Raises RuntimeError on failure.
+ *
+ * \param[in] pyaction_client The action client to use when sending the request.
+ * \param[in] pygoal_request The request message to send.
+ * \return sequence_number PyLong object representing the index of the sent request, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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 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, received response), or
+ * \return 2-tuple (None, None) if there is no response, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+rclpy_action_take_goal_response(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ TAKE_SERVICE_RESPONSE(goal)
+}
+
+/// Send an action result request.
+/**
+ * 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.
+ * \param[in] pyresult_request The request message to send.
+ * \return sequence_number PyLong object representing the index of the sent request, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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 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, received response), or
+ * \return 2-tuple (None, None) if there is no response, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+rclpy_action_take_result_response(PyObject * Py_UNUSED(self), PyObject * args)
+{
+ TAKE_SERVICE_RESPONSE(result);
+}
+
+/// Send an action cancel request.
+/**
+ * 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.
+ * \param[in] pycancel_request The request message to send.
+ * \return sequence_number PyLong object representing the index of the sent request, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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 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, received response), or
+ * \return 2-tuple (None, None) if there is no response, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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; \
+ if (!PyArg_ParseTuple(args, "OO", & pyaction_client, & pymsg_type)) { \
+ return NULL; \
+ } \
+ rcl_action_client_t * action_client = (rcl_action_client_t *)PyCapsule_GetPointer( \
+ pyaction_client, "rcl_action_client_t"); \
+ if (!action_client) { \
+ 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) { \
+ 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 */ \
+ Py_RETURN_NONE; \
+ } \
+ PyErr_Format(PyExc_RuntimeError, \
+ "Failed to take " #Type " with an action client: %s", rcl_get_error_string().str); \
+ rcl_reset_error(); \
+ return NULL; \
+ } \
+ 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.
+ * Raises RuntimeError on failure while taking a feedback message. Note, this does not include
+ * the case where there are no messages available.
+ *
+ * \param[in] pyaction_client Capsule pointing to the action client to process the message.
+ * \param[in] pyfeedback_type Instance of the feedback message type to take.
+ * \return Python message with all fields populated with received message, or
+ * \return None if there is nothing to take, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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.
+ * Raises RuntimeError on failure while taking a status message. Note, this does not include
+ * the case where there are no messages available.
+ *
+ * \param[in] pyaction_client Capsule pointing to the action client to process the message.
+ * \param[in] pystatus_type Instance of the status message type to take.
+ * \return Python message with all fields populated with received message, or
+ * \return None if there is nothing to take, or
+ * \return NULL if there is a failure.
+ */
+static PyObject *
+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."
+ },
+ {
+ "rclpy_action_wait_set_add", rclpy_action_wait_set_add, METH_VARARGS,
+ "Add an action entitiy to a wait set."
+ },
+ {
+ "rclpy_action_wait_set_get_num_entities", rclpy_action_wait_set_get_num_entities, METH_VARARGS,
+ "Get the number of wait set entities for an action entitity."
+ },
+ {
+ "rclpy_action_wait_set_is_ready", rclpy_action_wait_set_is_ready, METH_VARARGS,
+ "Check if an action entity has any sub-entities ready in a wait set."
+ },
+ {
+ "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."
+ },
+ {
+ "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."
+ },
+ {
+ "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."
+ },
+ {
+ "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 */
+};
+
+PyDoc_STRVAR(rclpy_action__doc__,
+ "ROS 2 Python Action library.");
+
+/// Define the Python module
+static struct PyModuleDef _rclpy_action_module = {
+ PyModuleDef_HEAD_INIT,
+ "_rclpy_action",
+ rclpy_action__doc__,
+ -1, /* -1 means that the module keeps state in global variables */
+ rclpy_action_methods,
+ NULL,
+ NULL,
+ NULL,
+ NULL
+};
+
+/// Init function of this module
+PyMODINIT_FUNC PyInit__rclpy_action(void)
+{
+ return PyModule_Create(&_rclpy_action_module);
+}
diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h
new file mode 100644
index 000000000..915e41f17
--- /dev/null
+++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h
@@ -0,0 +1,70 @@
+// Copyright 2016 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.
+#ifndef RCLPY_COMMON__COMMON_H_
+#define RCLPY_COMMON__COMMON_H_
+
+#include
+
+#include
+
+#include "rclpy_common/visibility_control.h"
+
+typedef void * create_ros_message_signature (void);
+typedef void destroy_ros_message_signature (void *);
+typedef bool convert_from_py_signature (PyObject *, void *);
+typedef PyObject * convert_to_py_signature (void *);
+
+/// Convert a C rmw_qos_profile_t into a Python QoSProfile object
+/**
+ * \param[in] void pointer to a rmw_qos_profile_t structure
+ * \return QoSProfile object
+ */
+RCLPY_COMMON_PUBLIC
+PyObject *
+rclpy_convert_to_py_qos_policy(void * profile);
+
+RCLPY_COMMON_PUBLIC
+void *
+get_capsule_pointer(PyObject * pymetaclass, const char * attr);
+
+RCLPY_COMMON_PUBLIC
+void *
+rclpy_create_from_py(PyObject * pymessage, destroy_ros_message_signature ** destroy_ros_message);
+
+/// Convert a ROS message from a Python type to a C type.
+/**
+ * Raises AttributeError if the Python message type is missing a required attribute.
+ * Raises MemoryError on a memory allocation failure.
+ *
+ * \param[in] pymessage The Python message to convert from.
+ * \param[out] destroy_ros_message The destructor function for finalizing the returned message.
+ * \return The C version of the input ROS message.
+ */
+RCLPY_COMMON_PUBLIC
+void *
+rclpy_convert_from_py(PyObject * pymessage, destroy_ros_message_signature ** destroy_ros_message);
+
+/// Convert a ROS message from a C type to a Python type.
+/**
+ * Raises AttributeError if the Python type is missing a required attribute.
+ *
+ * \param[in] message The C message to convert to a Python type
+ * \param[in] pyclass An instance of the Python type to convert to.
+ * \return The Python version of the input ROS message.
+ */
+RCLPY_COMMON_PUBLIC
+PyObject *
+rclpy_convert_to_py(void * message, PyObject * pyclass);
+
+#endif // RCLPY_COMMON__COMMON_H_
diff --git a/rclpy/src/rclpy_common/include/rclpy_common/visibility_control.h b/rclpy/src/rclpy_common/include/rclpy_common/visibility_control.h
new file mode 100644
index 000000000..f11b87720
--- /dev/null
+++ b/rclpy/src/rclpy_common/include/rclpy_common/visibility_control.h
@@ -0,0 +1,48 @@
+// 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.
+#ifndef RCLPY_COMMON__VISIBILITY_CONTROL_H_
+#define RCLPY_COMMON__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+// https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+ #ifdef __GNUC__
+ #define RCLPY_COMMON_EXPORT __attribute__ ((dllexport))
+ #define RCLPY_COMMON_IMPORT __attribute__ ((dllimport))
+ #else
+ #define RCLPY_COMMON_EXPORT __declspec(dllexport)
+ #define RCLPY_COMMON_IMPORT __declspec(dllimport)
+ #endif
+ #ifdef RCLPY_COMMON_BUILDING_DLL
+ #define RCLPY_COMMON_PUBLIC RCLPY_COMMON_EXPORT
+ #else
+ #define RCLPY_COMMON_PUBLIC RCLPY_COMMON_IMPORT
+ #endif
+ #define RCLPY_COMMON_PUBLIC_TYPE RCLPY_COMMON_PUBLIC
+ #define RCLPY_COMMON_LOCAL
+#else
+ #define RCLPY_COMMON_EXPORT __attribute__ ((visibility("default")))
+ #define RCLPY_COMMON_IMPORT
+ #if __GNUC__ >= 4
+ #define RCLPY_COMMON_PUBLIC __attribute__ ((visibility("default")))
+ #define RCLPY_COMMON_LOCAL __attribute__ ((visibility("hidden")))
+ #else
+ #define RCLPY_COMMON_PUBLIC
+ #define RCLPY_COMMON_LOCAL
+ #endif
+ #define RCLPY_COMMON_PUBLIC_TYPE
+#endif
+
+#endif // RCLPY_COMMON__VISIBILITY_CONTROL_H_
diff --git a/rclpy/src/rclpy_common/src/common.c b/rclpy/src/rclpy_common/src/common.c
new file mode 100644
index 000000000..a34fd3946
--- /dev/null
+++ b/rclpy/src/rclpy_common/src/common.c
@@ -0,0 +1,184 @@
+// Copyright 2016 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.
+
+#include "rclpy_common/common.h"
+
+PyObject *
+rclpy_convert_to_py_qos_policy(void * profile)
+{
+ PyObject * pyqos_module = PyImport_ImportModule("rclpy.qos");
+ if (!pyqos_module) {
+ return NULL;
+ }
+
+ PyObject * pyqos_policy_class = PyObject_GetAttrString(pyqos_module, "QoSProfile");
+ Py_DECREF(pyqos_module);
+ if (!pyqos_policy_class) {
+ return NULL;
+ }
+
+ PyObject * pyqos_profile = PyObject_CallObject(pyqos_policy_class, NULL);
+ Py_DECREF(pyqos_policy_class);
+ if (!pyqos_profile) {
+ return NULL;
+ }
+
+ rmw_qos_profile_t * qos_profile = (rmw_qos_profile_t *)profile;
+
+ PyObject * pyqos_depth = PyLong_FromSize_t(qos_profile->depth);
+ if (!pyqos_depth) {
+ Py_DECREF(pyqos_profile);
+ return NULL;
+ }
+
+ PyObject * pyqos_history = PyLong_FromUnsignedLong(qos_profile->history);
+ if (!pyqos_history) {
+ Py_DECREF(pyqos_profile);
+ Py_DECREF(pyqos_depth);
+ return NULL;
+ }
+
+ PyObject * pyqos_reliability = PyLong_FromUnsignedLong(qos_profile->reliability);
+ if (!pyqos_reliability) {
+ Py_DECREF(pyqos_profile);
+ Py_DECREF(pyqos_depth);
+ Py_DECREF(pyqos_history);
+ return NULL;
+ }
+
+ PyObject * pyqos_durability = PyLong_FromUnsignedLong(qos_profile->durability);
+ if (!pyqos_durability) {
+ Py_DECREF(pyqos_profile);
+ Py_DECREF(pyqos_depth);
+ Py_DECREF(pyqos_history);
+ Py_DECREF(pyqos_reliability);
+ return NULL;
+ }
+
+ PyObject * pyqos_avoid_ros_namespace_conventions =
+ PyBool_FromLong(qos_profile->avoid_ros_namespace_conventions);
+ if (!pyqos_avoid_ros_namespace_conventions) {
+ Py_DECREF(pyqos_profile);
+ Py_DECREF(pyqos_depth);
+ Py_DECREF(pyqos_history);
+ Py_DECREF(pyqos_reliability);
+ Py_DECREF(pyqos_durability);
+ return NULL;
+ }
+
+ // A success returns 0, and a failure returns -1
+ int set_result = 0;
+ set_result += PyObject_SetAttrString(pyqos_profile, "depth", pyqos_depth);
+ set_result += PyObject_SetAttrString(pyqos_profile, "history", pyqos_history);
+ set_result += PyObject_SetAttrString(pyqos_profile, "reliability", pyqos_reliability);
+ set_result += PyObject_SetAttrString(pyqos_profile, "durability", pyqos_durability);
+ set_result += PyObject_SetAttrString(pyqos_profile,
+ "avoid_ros_namespace_conventions", pyqos_avoid_ros_namespace_conventions);
+
+ Py_DECREF(pyqos_depth);
+ Py_DECREF(pyqos_history);
+ Py_DECREF(pyqos_reliability);
+ Py_DECREF(pyqos_durability);
+ Py_DECREF(pyqos_avoid_ros_namespace_conventions);
+
+ if (0 != set_result) {
+ Py_DECREF(pyqos_profile);
+ return NULL;
+ }
+ return pyqos_profile;
+}
+
+void *
+get_capsule_pointer(PyObject * pymetaclass, const char * attr)
+{
+ PyObject * pyattr = PyObject_GetAttrString(pymetaclass, attr);
+ if (!pyattr) {
+ return NULL;
+ }
+ void * ptr = PyCapsule_GetPointer(pyattr, NULL);
+ Py_DECREF(pyattr);
+ return ptr;
+}
+
+void *
+rclpy_create_from_py(PyObject * pymessage, destroy_ros_message_signature ** destroy_ros_message)
+{
+ PyObject * pymetaclass = PyObject_GetAttrString(pymessage, "__class__");
+ if (!pymetaclass) {
+ return NULL;
+ }
+
+ create_ros_message_signature * create_ros_message = get_capsule_pointer(
+ pymetaclass, "_CREATE_ROS_MESSAGE");
+ if (!create_ros_message) {
+ Py_DECREF(pymetaclass);
+ return NULL;
+ }
+
+ *destroy_ros_message = get_capsule_pointer(
+ pymetaclass, "_DESTROY_ROS_MESSAGE");
+ Py_DECREF(pymetaclass);
+ if (!destroy_ros_message) {
+ return NULL;
+ }
+
+ void * message = create_ros_message();
+ if (!message) {
+ return PyErr_NoMemory();
+ }
+ return message;
+}
+
+void *
+rclpy_convert_from_py(PyObject * pymessage, destroy_ros_message_signature ** destroy_ros_message)
+{
+ void * message = rclpy_create_from_py(pymessage, destroy_ros_message);
+ if (!message) {
+ return NULL;
+ }
+
+ PyObject * pymetaclass = PyObject_GetAttrString(pymessage, "__class__");
+ if (!pymetaclass) {
+ return NULL;
+ }
+
+ convert_from_py_signature * convert = get_capsule_pointer(
+ pymetaclass, "_CONVERT_FROM_PY");
+ Py_DECREF(pymetaclass);
+ if (!convert) {
+ return NULL;
+ }
+
+ if (!convert(pymessage, message)) {
+ (**destroy_ros_message)(message);
+ return NULL;
+ }
+ return message;
+}
+
+PyObject *
+rclpy_convert_to_py(void * message, PyObject * pyclass)
+{
+ PyObject * pymetaclass = PyObject_GetAttrString(pyclass, "__class__");
+ if (!pymetaclass) {
+ return NULL;
+ }
+ convert_to_py_signature * convert = get_capsule_pointer(
+ pymetaclass, "_CONVERT_TO_PY");
+ Py_DECREF(pymetaclass);
+ if (!convert) {
+ return NULL;
+ }
+ return convert(message);
+}
diff --git a/rclpy/test/action/test_client.py b/rclpy/test/action/test_client.py
new file mode 100644
index 000000000..eec510c07
--- /dev/null
+++ b/rclpy/test/action/test_client.py
@@ -0,0 +1,329 @@
+# 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
+
+import rclpy
+from rclpy.action import ActionClient
+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
+
+
+# TODO(jacobperron) Reduce fudge once wait_for_service uses node graph events
+TIME_FUDGE = 0.3
+
+
+class MockActionServer():
+
+ def __init__(self, node):
+ self.goal_srv = node.create_service(
+ Fibonacci.GoalRequestService, '/fibonacci/_action/send_goal', self.goal_callback)
+ self.cancel_srv = node.create_service(
+ Fibonacci.CancelGoalService, '/fibonacci/_action/cancel_goal', self.cancel_callback)
+ self.result_srv = node.create_service(
+ Fibonacci.GoalResultService, '/fibonacci/_action/get_result', self.result_callback)
+ self.feedback_pub = node.create_publisher(
+ Fibonacci.Feedback, '/fibonacci/_action/feedback')
+
+ def goal_callback(self, request, response):
+ response.accepted = True
+ return response
+
+ def cancel_callback(self, request, response):
+ response.goals_canceling.append(request.goal_info)
+ return response
+
+ def result_callback(self, request, response):
+ return response
+
+ def publish_feedback(self, goal_id):
+ feedback = Fibonacci.Feedback()
+ feedback.action_goal_id = goal_id
+ self.feedback_pub.publish(feedback)
+
+
+class TestActionClient(unittest.TestCase):
+
+ @classmethod
+ def setUpClass(cls):
+ cls.context = rclpy.context.Context()
+ rclpy.init(context=cls.context)
+ cls.executor = SingleThreadedExecutor(context=cls.context)
+ cls.node = rclpy.create_node('TestActionClient', context=cls.context)
+ cls.mock_action_server = MockActionServer(cls.node)
+
+ @classmethod
+ def tearDownClass(cls):
+ cls.node.destroy_node()
+ rclpy.shutdown(context=cls.context)
+
+ def setUp(self):
+ self.feedback = None
+
+ def feedback_callback(self, feedback):
+ self.feedback = feedback
+
+ 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 test_constructor_defaults(self):
+ # Defaults
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ ac.destroy()
+
+ def test_constructor_no_defaults(self):
+ ac = ActionClient(
+ self.node,
+ Fibonacci,
+ 'fibonacci',
+ 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_sub_qos_profile=rclpy.qos.qos_profile_default,
+ status_sub_qos_profile=rclpy.qos.qos_profile_default
+ )
+ ac.destroy()
+
+ def test_get_num_entities(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ num_entities = ac.get_num_entities()
+ self.assertEqual(num_entities.num_subscriptions, 2)
+ self.assertEqual(num_entities.num_guard_conditions, 0)
+ self.assertEqual(num_entities.num_timers, 0)
+ self.assertEqual(num_entities.num_clients, 3)
+ self.assertEqual(num_entities.num_services, 0)
+ ac.destroy()
+
+ def test_wait_for_server_nowait(self):
+ ac = ActionClient(self.node, Fibonacci, 'not_fibonacci')
+ try:
+ start = time.monotonic()
+ self.assertFalse(ac.wait_for_server(timeout_sec=0.0))
+ end = time.monotonic()
+ self.assertGreater(0.0, end - start - TIME_FUDGE)
+ self.assertLess(0.0, end - start + TIME_FUDGE)
+ finally:
+ ac.destroy()
+
+ def test_wait_for_server_timeout(self):
+ ac = ActionClient(self.node, Fibonacci, 'not_fibonacci')
+ try:
+ start = time.monotonic()
+ self.assertFalse(ac.wait_for_server(timeout_sec=2.0))
+ end = time.monotonic()
+ self.assertGreater(2.0, end - start - TIME_FUDGE)
+ self.assertLess(2.0, end - start + TIME_FUDGE)
+ finally:
+ ac.destroy()
+
+ def test_wait_for_server_exists(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ start = time.monotonic()
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+ end = time.monotonic()
+ self.assertGreater(0.0, end - start - TIME_FUDGE)
+ self.assertLess(0.0, end - start + TIME_FUDGE)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+ future = ac.send_goal_async(Fibonacci.Goal())
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+ self.assertTrue(future.done())
+ goal_handle = future.result()
+ self.assertTrue(goal_handle.accepted)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async_with_feedback_after_goal(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Send a goal and then publish feedback
+ goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
+ future = ac.send_goal_async(
+ Fibonacci.Goal(),
+ feedback_callback=self.feedback_callback,
+ goal_uuid=goal_uuid)
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+
+ # Publish feedback after goal has been accepted
+ self.mock_action_server.publish_feedback(goal_uuid)
+ self.timed_spin(1.0)
+ self.assertNotEqual(self.feedback, None)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async_with_feedback_before_goal(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Publish feedback before goal has been accepted
+ goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
+ self.mock_action_server.publish_feedback(goal_uuid)
+ self.timed_spin(1.0)
+ self.assertEqual(self.feedback, None)
+
+ # Send a goal and then publish feedback
+ future = ac.send_goal_async(
+ Fibonacci.Goal(),
+ feedback_callback=self.feedback_callback,
+ goal_uuid=goal_uuid)
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+
+ # Check the feedback was not received
+ self.assertEqual(self.feedback, None)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async_with_feedback_for_another_goal(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Send a goal and then publish feedback
+ first_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
+ future = ac.send_goal_async(
+ Fibonacci.Goal(),
+ feedback_callback=self.feedback_callback,
+ goal_uuid=first_goal_uuid)
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+
+ # Send another goal, but without a feedback callback
+ second_goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
+ future = ac.send_goal_async(
+ Fibonacci.Goal(),
+ goal_uuid=second_goal_uuid)
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+
+ # Publish feedback for the second goal
+ self.mock_action_server.publish_feedback(second_goal_uuid)
+ self.timed_spin(1.0)
+ self.assertEqual(self.feedback, None)
+ # Publish feedback for the first goal (with callback)
+ self.mock_action_server.publish_feedback(first_goal_uuid)
+ self.timed_spin(1.0)
+ self.assertNotEqual(self.feedback, None)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async_with_feedback_for_not_a_goal(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Send a goal and then publish feedback
+ goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
+ future = ac.send_goal_async(
+ Fibonacci.Goal(),
+ feedback_callback=self.feedback_callback,
+ goal_uuid=goal_uuid)
+ rclpy.spin_until_future_complete(self.node, future, self.executor)
+
+ # Publish feedback for a non-existent goal ID
+ self.mock_action_server.publish_feedback(UUID(uuid=list(uuid.uuid4().bytes)))
+ self.timed_spin(1.0)
+ self.assertEqual(self.feedback, None)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_multiple(self):
+ ac = ActionClient(
+ self.node,
+ Fibonacci,
+ 'fibonacci',
+ callback_group=ReentrantCallbackGroup())
+ executor = MultiThreadedExecutor(context=self.context)
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+ future_0 = ac.send_goal_async(Fibonacci.Goal())
+ future_1 = ac.send_goal_async(Fibonacci.Goal())
+ future_2 = ac.send_goal_async(Fibonacci.Goal())
+ rclpy.spin_until_future_complete(self.node, future_0, executor)
+ rclpy.spin_until_future_complete(self.node, future_1, executor)
+ rclpy.spin_until_future_complete(self.node, future_2, executor)
+ self.assertTrue(future_0.done())
+ self.assertTrue(future_1.done())
+ self.assertTrue(future_2.done())
+ self.assertTrue(future_0.result().accepted)
+ self.assertTrue(future_1.result().accepted)
+ self.assertTrue(future_2.result().accepted)
+ finally:
+ ac.destroy()
+
+ def test_send_goal_async_no_server(self):
+ ac = ActionClient(self.node, Fibonacci, 'not_fibonacci')
+ try:
+ future = ac.send_goal_async(Fibonacci.Goal())
+ self.timed_spin(2.0)
+ self.assertFalse(future.done())
+ finally:
+ ac.destroy()
+
+ def test_send_cancel_async(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Send a goal
+ goal_future = ac.send_goal_async(Fibonacci.Goal())
+ rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
+ self.assertTrue(goal_future.done())
+ goal_handle = goal_future.result()
+
+ # Cancel the goal
+ cancel_future = goal_handle.cancel_goal_async()
+ rclpy.spin_until_future_complete(self.node, cancel_future, self.executor)
+ self.assertTrue(cancel_future.done())
+ self.assertEqual(
+ cancel_future.result().goals_canceling[0].goal_id,
+ goal_handle.goal_id)
+ finally:
+ ac.destroy()
+
+ def test_get_result_async(self):
+ ac = ActionClient(self.node, Fibonacci, 'fibonacci')
+ try:
+ self.assertTrue(ac.wait_for_server(timeout_sec=2.0))
+
+ # Send a goal
+ goal_future = ac.send_goal_async(Fibonacci.Goal())
+ rclpy.spin_until_future_complete(self.node, goal_future, self.executor)
+ self.assertTrue(goal_future.done())
+ goal_handle = goal_future.result()
+
+ # Get the goal result
+ result_future = goal_handle.get_result_async()
+ rclpy.spin_until_future_complete(self.node, result_future, self.executor)
+ self.assertTrue(result_future.done())
+ finally:
+ ac.destroy()
+
+
+if __name__ == '__main__':
+ unittest.main()
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()
diff --git a/rclpy/test/test_executor.py b/rclpy/test/test_executor.py
index 460206b54..83e355135 100644
--- a/rclpy/test/test_executor.py
+++ b/rclpy/test/test_executor.py
@@ -13,6 +13,7 @@
# limitations under the License.
import asyncio
+import threading
import time
import unittest
@@ -221,6 +222,44 @@ async def coro2():
self.assertTrue(future2.done())
self.assertEqual('Sentinel Result 2', future2.result())
+ def test_create_task_during_spin(self):
+ self.assertIsNotNone(self.node.handle)
+ executor = SingleThreadedExecutor(context=self.context)
+ executor.add_node(self.node)
+
+ future = None
+
+ def spin_until_task_done(executor):
+ nonlocal future
+ while future is None or not future.done():
+ try:
+ executor.spin_once()
+ finally:
+ executor.shutdown()
+ break
+
+ # Start spinning in a separate thread
+ thr = threading.Thread(target=spin_until_task_done, args=(executor, ), daemon=True)
+ thr.start()
+
+ # Sleep in this thread to give the executor a chance to reach the loop in
+ # '_wait_for_ready_callbacks()'
+ time.sleep(1)
+
+ def func():
+ return 'Sentinel Result'
+
+ # Create a task
+ future = executor.create_task(func)
+
+ thr.join(timeout=0.5)
+ # If the join timed out, remove the node to cause the spin thread to stop
+ if thr.is_alive():
+ executor.remove_node(self.node)
+
+ self.assertTrue(future.done())
+ self.assertEqual('Sentinel Result', future.result())
+
def test_global_executor_completes_async_task(self):
self.assertIsNotNone(self.node.handle)
@@ -259,7 +298,9 @@ def test_executor_add_node(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
assert executor.add_node(self.node)
+ assert id(executor) == id(self.node.executor)
assert not executor.add_node(self.node)
+ assert id(executor) == id(self.node.executor)
if __name__ == '__main__':