Update action server examples

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

rclpy/actions/minimal_action_server/server.py

@@ -12,14 +12,15 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import time
+
 from example_interfaces.action import Fibonacci
 
 import rclpy
-from rclpy.action import ActionServer
-from rclpy.action import GoalResponse
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-import threading
 
 
 class MinimalActionServer(Node):
@@ -31,47 +32,60 @@ def __init__(self):
             self,
             Fibonacci,
             'fibonacci',
+            execute_callback=self.execute_callback,
             callback_group=ReentrantCallbackGroup(),
             goal_callback=self.goal_callback,
-            cancel_callback=self.cancel_callback,
-            execute_callback=self.execute_callback)
+            cancel_callback=self.cancel_callback)
 
     def destroy(self):
         self._action_server.destroy()
         super().destroy_node()
 
-    def goal_callback(self, goal):
+    def goal_callback(self, goal_request):
         """Accepts or rejects a client request to begin an action."""
         # This server allows multiple goals in parallel
-        return GoalResponse.ACCEPT
+        self.get_logger().info('Received goal request')
+        return GoalResponse.ACCEPT_AND_EXECUTE
 
-    def cancel_callback(self, goal):
+    def cancel_callback(self, goal_handle):
         """Accepts or rejects a client request to cancel an action."""
-        return GoalResponse.ACCEPT
+        self.get_logger().info('Received cancel request')
+        return CancelResponse.ACCEPT
 
-    async def execute_callback(self, goal):
+    async def execute_callback(self, goal_handle):
         """Executes a goal."""
-        feedback_msg = Fibonacci.Feedback()
+        self.get_logger().info('Executing goal...')
 
         # Append the seeds for the Fibonacci sequence
+        feedback_msg = Fibonacci.Feedback()
         feedback_msg.sequence = [0, 1]
 
         # Start executing the action
-        for i in range(1, goal.request.order):
-            if goal.is_cancel_requested():
-                result = Fibonacci.Result()
-                result.response = GoalResponse.CANCELLED
-                return result
-            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence([i-1]))
+        for i in range(1, goal_handle.request.order):
+            if goal_handle.is_cancel_requested:
+                goal_handle.set_canceled()
+                self.get_logger().info('Goal canceled')
+                return Fibonacci.Result()
+
+            # Update Fibonacci sequence
+            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
+
+            self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence))
+
             # Publish the feedback
-            goal.publish_feedback(feedback_msg)
+            goal_handle.publish_feedback(feedback_msg)
 
             # Sleep for demonstration purposes
-            asyncio.sleep(1)
+            time.sleep(1)
+
+        goal_handle.set_succeeded()
 
+        # Populate result message
         result = Fibonacci.Result()
-        result.result.sequence = feedback_msg.sequence
-        result.response = GoalResponse.SUCCEEDED
+        result.sequence = feedback_msg.sequence
+
+        self.get_logger().info('Returning result: {0}'.format(result.sequence))
+
         return result
 
 
@@ -80,7 +94,10 @@ def main(args=None):
 
     minimal_action_server = MinimalActionServer()
 
-    rclpy.spin(minimal_action_server)
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
+
+    rclpy.spin(minimal_action_server, executor=executor)
 
     minimal_action_server.destroy()
     rclpy.shutdown()

rclpy/actions/minimal_action_server/server_not_composable.py

@@ -12,56 +12,78 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import time
+
 from example_interfaces.action import Fibonacci
 
 import rclpy
 from rclpy.action import ActionServer
-from rclpy.action import GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 
 
-async def execute_callback(goal):
+logger = None
+
+
+async def execute_callback(goal_handle):
     """Executes the goal."""
-    feedback_msg = Fibonacci.Feedback()
+    logger.info('Executing goal...')
 
-    # append the seeds for the fibonacci sequence
+    # Append the seeds for the fibonacci sequence
+    feedback_msg = Fibonacci.Feedback()
     feedback_msg.sequence = [0, 1]
 
-    # start executing the action
-    for i in range(1, goal.request.order):
-        if goal.is_cancel_requested():
-            result = Fibonacci.Result()
-            result.response = GoalResponse.CANCELLED
-            return result
-        feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence([i-1]))
-        # publish the feedback
-        goal.publish_feedback(feedback_msg)
+    # Start executing the action
+    for i in range(1, goal_handle.request.order):
+        if goal_handle.is_cancel_requested:
+            goal_handle.set_canceled()
+            self.get_logger().info('Goal canceled')
+            return Fibonacci.Result()
+
+        # Update Fibonacci sequence
+        feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
+
+        logger.info('Publishing feedback: {0}'.format(feedback_msg.sequence))
+
+        # Publish feedback
+        goal_handle.publish_feedback(feedback_msg)
 
         # Sleep for demonstration purposes
-        asyncio.sleep(1)
+        time.sleep(1)
+
+    goal_handle.set_succeeded()
 
-    with g_lock:
-        g_goal = None
+    # Populate result message
     result = Fibonacci.Result()
-    result.result.sequence = feedback_msg.sequence
-    result.response = GoalResponse.SUCCEEDED
+    result.sequence = feedback_msg.sequence
+
+    logger.info('Returning result: {0}'.format(result.sequence))
+
     return result
 
 
 def main(args=None):
+    global logger
     rclpy.init(args=args)
 
     node = rclpy.create_node('minimal_action_server')
+    logger = node.get_logger()
 
+    # Use a ReentrantCallbackGroup to enable processing multiple goals concurrently
+    # Default goal callback accepts all goals
+    # Default cancel callback rejects cancel requests
     action_server = ActionServer(
         node,
         Fibonacci,
         'fibonacci',
-        goal_callback=lambda g: GoalResponse.ACCEPT,
-        cancel_callback=lambda g: GoalResponse.ACCEPT,
-        execute_callback=execute_callback)
+        execute_callback=execute_callback,
+        callback_group=ReentrantCallbackGroup())
+
+    # Use a MultiThreadedExecutor to enable processing goals concurrently
+    executor = MultiThreadedExecutor()
 
-    rclpy.spin(node)
+    rclpy.spin(node, executor=executor)
 
     action_server.destroy()
     node.destroy_node()

rclpy/actions/minimal_action_server/server_single_goal.py

@@ -12,73 +12,97 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import threading
+import time
+
 from example_interfaces.action import Fibonacci
 
 import rclpy
-from rclpy.action import ActionServer
-from rclpy.action import GoalResponse
+from rclpy.action import ActionServer, CancelResponse, GoalResponse
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-import threading
 
 
 class MinimalActionServer(Node):
     """Minimal action server that processes one goal at a time."""
 
     def __init__(self):
         super().__init__('minimal_action_server')
-        self._goal = None
+        self._goal_handle = None
         self._goal_lock = threading.Lock()
         self._action_server = ActionServer(
             self,
             Fibonacci,
             'fibonacci',
+            execute_callback=self.execute_callback,
             goal_callback=self.goal_callback,
             cancel_callback=self.cancel_callback,
-            execute_callback=self.execute_callback)
+            callback_group=ReentrantCallbackGroup())
 
     def destroy(self):
         self._action_server.destroy()
         super().destroy_node()
 
-    def goal_callback(self, goal):
+    def goal_callback(self, goal_request):
         """Accepts or rejects a client request to begin an action."""
+        self.get_logger().info('Received goal request')
         with self._goal_lock:
             # This server only allows one goal at a time
-            if self._goal is not None:
-                # Preempt existing goal
-                self._goal.cancel()
-            self._goal = goal
-        return GoalResponse.ACCEPT
+            if self._goal_handle is not None and self._goal_handle.is_active:
+                self.get_logger().info('Aborting previous goal')
+                # Abort the existing goal
+                self._goal_handle.set_aborted()
+
+        return GoalResponse.ACCEPT_AND_EXECUTE
 
     def cancel_callback(self, goal):
         """Accepts or rejects a client request to cancel an action."""
-        return GoalResponse.ACCEPT
+        self.get_logger().info('Received cancel request')
+        return CancelResponse.ACCEPT
 
-    async def execute_callback(self, goal):
+    def execute_callback(self, goal_handle):
         """Executes the goal."""
-        feedback_msg = Fibonacci.Feedback()
+        self.get_logger().info('Executing goal...')
+        with self._goal_lock:
+            self._goal_handle = goal_handle
 
         # Append the seeds for the Fibonacci sequence
+        feedback_msg = Fibonacci.Feedback()
         feedback_msg.sequence = [0, 1]
 
         # Start executing the action
-        for i in range(1, goal.request.order):
-            if goal.is_cancel_requested():
-                result = Fibonacci.Result()
-                result.response = GoalResponse.CANCELLED
-                return result
-            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence([i-1]))
+        for i in range(1, goal_handle.request.order):
+            # If goal is flagged as no longer active (ie. another goal was accepted),
+            # then stop executing
+            if not goal_handle.is_active:
+                self.get_logger().info('Goal aborted')
+                return Fibonacci.Result()
+
+            if goal_handle.is_cancel_requested:
+                goal_handle.set_canceled()
+                self.get_logger().info('Goal canceled')
+                return Fibonacci.Result()
+
+            # Update Fibonacci sequence
+            feedback_msg.sequence.append(feedback_msg.sequence[i] + feedback_msg.sequence[i-1])
+
+            self.get_logger().info('Publishing feedback: {0}'.format(feedback_msg.sequence))
+
             # Publish the feedback
-            goal.publish_feedback(feedback_msg)
+            goal_handle.publish_feedback(feedback_msg)
 
             # Sleep for demonstration purposes
-            asyncio.sleep(1)
+            time.sleep(1)
 
-        with self._goal_lock:
-            self._goal = None
+        goal_handle.set_succeeded()
+
+        # Populate result message
         result = Fibonacci.Result()
-        result.result.sequence = feedback_msg.sequence
-        result.response = GoalResponse.SUCCEEDED
+        result.sequence = feedback_msg.sequence
+
+        self.get_logger().info('Returning result: {0}'.format(result.sequence))
+
         return result
 
 
@@ -87,7 +111,9 @@ def main(args=None):
 
     action_server = MinimalActionServer()
 
-    rclpy.spin(action_server)
+    # We use a MultiThreadedExecutor to handle incoming goal requests concurrently
+    executor = MultiThreadedExecutor()
+    rclpy.spin(action_server, executor=executor)
 
     action_server.destroy()
     rclpy.shutdown()