Add Action Client

rclpy/rclpy/action.py → rclpy/rclpy/action/__init__.py

@@ -12,4 +12,4 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from rclpy.action_client import ActionClient  # noqa
+from .client import ActionClient  # noqa

rclpy/rclpy/action_client.py → rclpy/rclpy/action/client.py

@@ -19,13 +19,12 @@
 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
-# TODO(jacobperron): Move check_for_type_support to its own module (e.g. type_support)
-#                    Do after Crystal patch release since this breaks API
-from rclpy.node import check_for_type_support
 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
@@ -34,17 +33,18 @@
 class ClientGoalHandle():
     """Goal handle for working with Action Clients."""
 
-    def __init__(self, goal_id, goal_response):
-        if not isinstance(goal_id, UUID):
-            raise TypeError('Expected UUID, but given {}'.format(type(goal_id)))
-
+    def __init__(self, action_server, goal_id, goal_response):
+        self._action_server = action_server
         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 <id={0}, accepted={1}, status={2}>'.format(
                 self.goal_id.uuid,
@@ -56,8 +56,8 @@ def goal_id(self):
         return self._goal_id
 
     @property
-    def goal_response(self):
-        return self._goal_response
+    def stamp(self):
+        return self._goal_response.stamp
 
     @property
     def accepted(self):
@@ -67,6 +67,44 @@ def accepted(self):
     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_server._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_server._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_server._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_server._get_result_async(self)
+
 
 class ActionClient(Waitable):
     """ROS Action client."""
@@ -120,11 +158,18 @@ def __init__(
         )
 
         self._is_ready = False
+
+        # key: UUID in bytes, value: ClientGoalHandle
         self._goal_handles = {}
+        # key: goal request sequence_number, value: Future for goal response
         self._pending_goal_requests = {}
-        self._sequence_number_to_goal_id = {}  # goal request sequence number
+        # 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)
@@ -139,14 +184,14 @@ def _remove_pending_request(self, future, 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`.
+            :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 pending_requests.items():
+        for seq, req_future in list(pending_requests.items()):
             if future == req_future:
                 try:
                     del pending_requests[seq]
@@ -204,7 +249,7 @@ def take_data(self):
             data['status'] = _rclpy_action.rclpy_action_take_status(
                 self._client_handle, self._action_type.GoalStatusMessage)
 
-        if not any(data):
+        if not data:
             return None
         return data
 
@@ -218,6 +263,7 @@ async def execute(self, taken_data):
         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)
 
@@ -240,10 +286,10 @@ async def execute(self, taken_data):
 
         if 'feedback' in taken_data:
             feedback_msg = taken_data['feedback']
-            goal_uuid = uuid.UUID(bytes=bytes(feedback_msg.action_goal_id.uuid))
+            goal_uuid = bytes(feedback_msg.action_goal_id.uuid)
             # Call a registered callback if there is one
             if goal_uuid in self._feedback_callbacks:
-                self._feedback_callbacks[goal_uuid](feedback_msg)
+                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
@@ -262,12 +308,7 @@ async def execute(self, taken_data):
     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[0],
-            num_entities[1],
-            num_entities[2],
-            num_entities[3],
-            num_entities[4])
+        return NumberOfEntities(*num_entities)
 
     def add_to_wait_set(self, wait_set):
         """Add entities to wait set."""
@@ -305,7 +346,7 @@ def unblock(future):
 
         goal_handle = send_goal_future.result()
 
-        result = self.get_result(goal_handle)
+        result = self._get_result(goal_handle)
 
         return result
 
@@ -335,7 +376,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
 
         if feedback_callback is not None:
             # TODO(jacobperron): Move conversion function to a general-use package
-            goal_uuid = uuid.UUID(bytes=bytes(goal.action_goal_id.uuid))
+            goal_uuid = bytes(goal.action_goal_id.uuid)
             self._feedback_callbacks[goal_uuid] = feedback_callback
 
         future = Future()
@@ -347,7 +388,7 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
 
         return future
 
-    def cancel_goal(self, goal_handle):
+    def _cancel_goal(self, goal_handle):
         """
         Send a cancel request for an active goal and wait for the response.
 
@@ -363,15 +404,15 @@ def unblock(future):
             nonlocal event
             event.set()
 
-        future = self.cancel_goal_async(goal_handle)
+        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):
+    def _cancel_goal_async(self, goal_handle):
         """
         Send a cancel request for an active goal and asynchronously get the result.
 
@@ -401,7 +442,7 @@ def cancel_goal_async(self, goal_handle):
 
         return future
 
-    def get_result(self, goal_handle):
+    def _get_result(self, goal_handle):
         """
         Request the result for an active goal and wait for the response.
 
@@ -417,15 +458,15 @@ def unblock(future):
             nonlocal event
             event.set()
 
-        future = self.get_result_async(goal_handle)
+        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):
+    def _get_result_async(self, goal_handle):
         """
         Request the result for an active goal asynchronously.
 

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__(

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()

rclpy/src/rclpy/impl/common.h

@@ -37,6 +37,7 @@ rclpy_convert_to_py_qos_policy(void * profile)
   }
 
   PyObject * pyqos_policy_class = PyObject_GetAttrString(pyqos_module, "QoSProfile");
+  Py_DECREF(pyqos_module);
   if (!pyqos_policy_class) {
     return NULL;
   }
@@ -99,12 +100,13 @@ rclpy_convert_to_py_qos_policy(void * profile)
   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_depth);
-    Py_DECREF(pyqos_history);
-    Py_DECREF(pyqos_reliability);
-    Py_DECREF(pyqos_durability);
-    Py_DECREF(pyqos_avoid_ros_namespace_conventions);
     Py_DECREF(pyqos_profile);
     return NULL;
   }