import rclpy from rclpy import Future from rclpy.action import ActionServer, ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.action.server import ServerGoalHandle import rclpy.logging from rclpy.node import Node from action_tutorials_interfaces.action import Fibonacci from rclpy.executors import ExternalShutdownException from rclpy.executors import MultiThreadedExecutor class FibonacciActionServer(Node): def __init__(self, executor): super().__init__("fibonacci_action_server") self._action_server = ActionServer( self, Fibonacci, "some_action", self.execute_callback ) self._action_client = ActionClient( self, Fibonacci, "fibonacci" ) self._executor = executor def execute_callback(self, goal_handle: ServerGoalHandle): self.get_logger().info("Requesting goal...") # self._action_client.send_goal_async() request_future = self._action_client.send_goal_async(Fibonacci.Goal(order=goal_handle.request.order)) #rclpy.spin_until_future_complete(self,request_future) self._executor.spin_until_future_complete(request_future) self.get_logger().info("Recieved request result...") spin_handle: ClientGoalHandle = request_future.result() result_future: Future = spin_handle.get_result_async() #rclpy.spin_until_future_complete(self,result_future) self._executor.spin_until_future_complete(result_future) self.get_logger().info("Recieved final result...") result = Fibonacci.Result() if not result_future.cancelled(): result = result_future.result().result goal_handle.succeed() return result def main(args=None): try: with rclpy.init(args=args): executor = MultiThreadedExecutor(num_threads=4) fibonacci_action_server = FibonacciActionServer(executor) executor.add_node(fibonacci_action_server) executor.spin() except (KeyboardInterrupt, ExternalShutdownException): pass if __name__ == '__main__': main()