diff --git a/rclpy/actions/minimal_action_server/server.py b/rclpy/actions/minimal_action_server/server.py index 1c94213a..0a863a5f 100644 --- a/rclpy/actions/minimal_action_server/server.py +++ b/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() diff --git a/rclpy/actions/minimal_action_server/server_not_composable.py b/rclpy/actions/minimal_action_server/server_not_composable.py index 131246bc..338e4e2b 100644 --- a/rclpy/actions/minimal_action_server/server_not_composable.py +++ b/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() diff --git a/rclpy/actions/minimal_action_server/server_single_goal.py b/rclpy/actions/minimal_action_server/server_single_goal.py index 08929aa0..a6b4e1e2 100644 --- a/rclpy/actions/minimal_action_server/server_single_goal.py +++ b/rclpy/actions/minimal_action_server/server_single_goal.py @@ -12,13 +12,16 @@ # 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): @@ -26,59 +29,80 @@ class MinimalActionServer(Node): 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()