diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt
index d6c6e0764..d8427de69 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)
@@ -82,6 +83,17 @@ ament_target_dependencies(rclpy
"rcutils"
)
+add_library(
+ rclpy_action
+ SHARED src/rclpy/_rclpy_action.c
+)
+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 90555fb5c..34a4e3acd 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..5e2b05413
--- /dev/null
+++ b/rclpy/rclpy/action/__init__.py
@@ -0,0 +1,15 @@
+# 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
diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py
new file mode 100644
index 000000000..7dcf055c7
--- /dev/null
+++ b/rclpy/rclpy/action/client.py
@@ -0,0 +1,545 @@
+# 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:
+ data['goal'] = _rclpy_action.rclpy_action_take_goal_response(
+ self._client_handle, self._action_type.GoalRequestService.Response)
+
+ if self._is_cancel_response_ready:
+ data['cancel'] = _rclpy_action.rclpy_action_take_cancel_response(
+ self._client_handle, self._action_type.CancelGoalService.Response)
+
+ if self._is_result_response_ready:
+ data['result'] = _rclpy_action.rclpy_action_take_result_response(
+ self._client_handle, self._action_type.GoalResultService.Response)
+
+ if self._is_feedback_ready:
+ data['feedback'] = _rclpy_action.rclpy_action_take_feedback(
+ self._client_handle, self._action_type.Feedback)
+
+ if self._is_status_ready:
+ data['status'] = _rclpy_action.rclpy_action_take_status(
+ self._client_handle, self._action_type.GoalStatusMessage)
+
+ if not data:
+ return None
+ return data
+
+ async def execute(self, taken_data):
+ """
+ Execute work after data has been taken from a ready wait set.
+
+ This will set results for Future objects for any received service responses and
+ call any user-defined callbacks (e.g. feedback).
+ """
+ if 'goal' in taken_data:
+ 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/executors.py b/rclpy/rclpy/executors.py
index 127c3dac6..8b5cf600f 100644
--- a/rclpy/rclpy/executors.py
+++ b/rclpy/rclpy/executors.py
@@ -287,6 +287,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.
@@ -454,9 +459,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..7000a7cb7 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__(
@@ -139,6 +123,10 @@ def executor(self, new_executor):
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 +242,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 +265,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 +290,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 +313,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 +322,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 7bbefdaf7..b64816120 100644
--- a/rclpy/src/rclpy/_rclpy.c
+++ b/rclpy/src/rclpy/_rclpy.c
@@ -38,6 +38,8 @@
#include
+#include "./impl/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)
{
@@ -3427,34 +3413,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..072d83eb2
--- /dev/null
+++ b/rclpy/src/rclpy/_rclpy_action.c
@@ -0,0 +1,939 @@
+// 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 "./impl/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;
+}
+
+/// 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);
+}
+
+/// 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; \
+ } \
+ PyObject * pyrequest_type = PyObject_GetAttrString(pyrequest, "__class__"); \
+ if (!pyrequest_type) { \
+ return NULL; \
+ } \
+ PyObject * pymetaclass = PyObject_GetAttrString(pyrequest_type, "__class__"); \
+ Py_DECREF(pyrequest_type); \
+ if (!pymetaclass) { \
+ return NULL; \
+ } \
+ create_ros_message_signature * create_ros_message = get_capsule_pointer( \
+ pymetaclass, "_CREATE_ROS_MESSAGE"); \
+ assert(create_ros_message != NULL && \
+ "unable to retrieve create_ros_message function, type_support must not have been imported"); \
+ destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \
+ pymetaclass, "_DESTROY_ROS_MESSAGE"); \
+ assert(destroy_ros_message != NULL && \
+ "unable to retrieve destroy_ros_message function, type_support must not have been imported"); \
+ convert_from_py_signature * convert_from_py = get_capsule_pointer( \
+ pymetaclass, "_CONVERT_FROM_PY"); \
+ assert(convert_from_py != NULL && \
+ "unable to retrieve convert_from_py function, type_support must not have been imported"); \
+ Py_DECREF(pymetaclass); \
+ void * raw_ros_request = create_ros_message(); \
+ if (!raw_ros_request) { \
+ return PyErr_NoMemory(); \
+ } \
+ if (!convert_from_py(pyrequest, raw_ros_request)) { \
+ /* the function has set the Python error */ \
+ destroy_ros_message(raw_ros_request); \
+ return NULL; \
+ } \
+ int64_t sequence_number; \
+ 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 TAKE_SERVICE_RESPONSE(Type) \
+ PyObject * pyaction_client; \
+ PyObject * pyresponse_type; \
+ if (!PyArg_ParseTuple(args, "OO", & pyaction_client, & pyresponse_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; \
+ } \
+ PyObject * pymetaclass = PyObject_GetAttrString(pyresponse_type, "__class__"); \
+ if (!pymetaclass) { \
+ return NULL; \
+ } \
+ create_ros_message_signature * create_ros_message = get_capsule_pointer( \
+ pymetaclass, "_CREATE_ROS_MESSAGE"); \
+ assert(create_ros_message != NULL && \
+ "unable to retrieve create_ros_message function, type_support mustn't have been imported"); \
+ destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \
+ pymetaclass, "_DESTROY_ROS_MESSAGE"); \
+ assert(destroy_ros_message != NULL && \
+ "unable to retrieve destroy_ros_message function, type_support mustn't have been imported"); \
+ void * taken_response = create_ros_message(); \
+ if (!taken_response) { \
+ /* the function has set the Python error */ \
+ Py_DECREF(pymetaclass); \
+ return NULL; \
+ } \
+ rmw_request_id_t * header = (rmw_request_id_t *)PyMem_Malloc(sizeof(rmw_request_id_t)); \
+ if (!header) { \
+ Py_DECREF(pymetaclass); \
+ return PyErr_NoMemory(); \
+ } \
+ rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, header, taken_response); \
+ int64_t sequence = header->sequence_number; \
+ PyMem_Free(header); \
+ /* Create the tuple to return */ \
+ PyObject * pytuple = PyTuple_New(2); \
+ if (!pytuple) { \
+ Py_DECREF(pymetaclass); \
+ 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); \
+ Py_DECREF(pymetaclass); \
+ destroy_ros_message(taken_response); \
+ return pytuple; \
+ } \
+ convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY"); \
+ Py_DECREF(pymetaclass); \
+ PyObject * pytaken_response = convert_to_py(taken_response); \
+ destroy_ros_message(taken_response); \
+ if (!pytaken_response) { \
+ /* the function has set the Python error */ \
+ Py_DECREF(pytuple); \
+ return NULL; \
+ } \
+ PyObject * pysequence = PyLong_FromLongLong(sequence); \
+ if (!pysequence) { \
+ Py_DECREF(pytaken_response); \
+ Py_DECREF(pytuple); \
+ return NULL; \
+ } \
+ PyTuple_SET_ITEM(pytuple, 0, pysequence); \
+ PyTuple_SET_ITEM(pytuple, 1, pytaken_response); \
+ return pytuple; \
+
+/// Send an action goal request.
+/**
+ * Raises AttributeError if there is an issue parsing the pygoal_request type.
+ * 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 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 and received response, or
+ * \return 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 type.
+ * 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 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 and received response, or
+ * \return 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 type.
+ * 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 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 and received response, or
+ * \return 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 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; \
+ } \
+ PyObject * pymetaclass = PyObject_GetAttrString(pymsg_type, "__class__"); \
+ if (!pymetaclass) { \
+ return NULL; \
+ } \
+ create_ros_message_signature * create_ros_message = get_capsule_pointer( \
+ pymetaclass, "_CREATE_ROS_MESSAGE"); \
+ assert(create_ros_message != NULL && \
+ "unable to retrieve create_ros_message function, type_support must not have been imported"); \
+ destroy_ros_message_signature * destroy_ros_message = get_capsule_pointer( \
+ pymetaclass, "_DESTROY_ROS_MESSAGE"); \
+ assert(destroy_ros_message != NULL && \
+ "unable to retrieve destroy_ros_message function, type_support must not have been imported"); \
+ convert_to_py_signature * convert_to_py = get_capsule_pointer(pymetaclass, "_CONVERT_TO_PY"); \
+ Py_DECREF(pymetaclass); \
+ void * taken_msg = create_ros_message(); \
+ if (!taken_msg) { \
+ return PyErr_NoMemory(); \
+ } \
+ rcl_ret_t ret = rcl_action_take_ ## Type(action_client, taken_msg); \
+ if (ret != RCL_RET_OK) { \
+ if (ret != RCL_RET_ACTION_CLIENT_TAKE_FAILED) { \
+ /* if take failed, just do nothing */ \
+ destroy_ros_message(taken_msg); \
+ Py_RETURN_NONE; \
+ } \
+ PyErr_Format(PyExc_RuntimeError, \
+ "Failed to take " #Type " with an action client: %s", rcl_get_error_string().str); \
+ rcl_reset_error(); \
+ destroy_ros_message(taken_msg); \
+ return NULL; \
+ } \
+ PyObject * pytaken_msg = convert_to_py(taken_msg); \
+ destroy_ros_message(taken_msg); \
+ return pytaken_msg;
+
+/// 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)
+}
+
+/// 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)
+}
+
+/// 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_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_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_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_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_response", rclpy_action_take_cancel_response, METH_VARARGS,
+ "Take a cancel response."
+ },
+ {
+ "rclpy_action_take_feedback", rclpy_action_take_feedback, METH_VARARGS,
+ "Take a feedback message."
+ },
+ {
+ "rclpy_action_take_status", rclpy_action_take_status, METH_VARARGS,
+ "Take a status message."
+ },
+
+ {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/impl/common.h b/rclpy/src/rclpy/impl/common.h
new file mode 100644
index 000000000..f28422d19
--- /dev/null
+++ b/rclpy/src/rclpy/impl/common.h
@@ -0,0 +1,127 @@
+// 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__IMPL__COMMON_H_
+#define RCLPY__IMPL__COMMON_H_
+
+#include
+
+#include
+
+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
+ */
+static 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;
+}
+
+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;
+}
+#endif // RCLPY__IMPL__COMMON_H_
diff --git a/rclpy/test/action/test_client.py b/rclpy/test/action/test_client.py
new file mode 100644
index 000000000..9578c4b20
--- /dev/null
+++ b/rclpy/test/action/test_client.py
@@ -0,0 +1,332 @@
+# 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.executors import 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()
+ current_time = start_time
+ while (current_time - start_time) < duration:
+ rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.1)
+ current_time = time.time()
+
+ def test_constructor_defaults(self):
+ # Defaults
+ 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()
+
+ # TODO(jacobperron): Figure out why this test gets stuck on some CI instances
+ # Related issue: https://github.com/ros2/rclpy/issues/268
+ # 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()