Disabled result caching: Result gets erased before result callback is…

… registered

  ros2/rclcpp#2101

Signed-off-by: Tomoya Fujita <[email protected]>

prover_rclcpp/CMakeLists.txt

@@ -42,6 +42,7 @@ find_package(std_msgs REQUIRED)
 find_package(prover_interfaces REQUIRED)
 find_package(example_interfaces REQUIRED)
 find_package(action_tutorials_interfaces REQUIRED)
+find_package(action_tutorials_cpp REQUIRED)
 
 function(custom_executable target)
   add_executable(${target} src/${target}.cpp)
@@ -54,7 +55,8 @@ function(custom_executable target)
     "lifecycle_msgs"
     "rclcpp_lifecycle"
     "example_interfaces"
-    "action_tutorials_interfaces")
+    "action_tutorials_interfaces"
+    "action_tutorials_cpp")
   target_compile_options(${target}
     PRIVATE "-Wall" "-Wextra" "-Werror" "-Wpedantic" "-pthread")
   target_link_options(${target}
@@ -100,6 +102,7 @@ custom_executable(rclcpp_1953)
 custom_executable(rclcpp_1968)
 custom_executable(rclcpp_2053)
 custom_executable(rclcpp_2062)
+custom_executable(rclcpp_2101_server)
 custom_executable(rclcpp_2138)
 custom_executable(rclcpp_2144)
 custom_executable(rclcpp_2146)

prover_rclcpp/package.xml

@@ -26,6 +26,7 @@
   <build_depend>prover_interfaces</build_depend>
   <build_depend>example_interfaces</build_depend>
   <build_depend>action_tutorials_interfaces</build_depend>
+  <build_depend>action_tutorials_cpp</build_depend>
 
   <exec_depend>lifecycle_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
@@ -39,6 +40,7 @@
   <exec_depend>prover_interfaces</exec_depend>
   <exec_depend>example_interfaces</exec_depend>
   <exec_depend>action_tutorials_interfaces</exec_depend>
+  <exec_depend>action_tutorials_cpp</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

prover_rclcpp/src/rclcpp_2101_server.cpp

@@ -0,0 +1,78 @@
+#include <functional>
+#include <memory>
+#include <thread>
+
+#include "action_tutorials_interfaces/action/fibonacci.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class FibonacciActionServer : public rclcpp::Node
+{
+public:
+  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
+  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+  : Node("fibonacci_action_server", options)
+  {
+    using namespace std::placeholders;
+
+    auto serverOptions = rcl_action_server_get_default_options();
+    serverOptions.result_timeout.nanoseconds = 0;
+
+    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
+      this,
+      "fibonacci",
+      std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
+      std::bind(&FibonacciActionServer::handle_cancel, this, _1),
+      std::bind(&FibonacciActionServer::handle_accepted, this, _1),
+      serverOptions);
+  }
+
+private:
+  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
+
+  rclcpp_action::GoalResponse handle_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const Fibonacci::Goal> goal)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse handle_cancel(
+    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
+  {
+    using namespace std::placeholders;
+    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
+    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
+  }
+
+  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    const auto goal = goal_handle->get_goal();
+    auto result = std::make_shared<Fibonacci::Result>();
+    result->sequence.push_back(42);
+    goal_handle->succeed(result);
+    RCLCPP_INFO(this->get_logger(), "Executing done");
+  }
+};  // class FibonacciActionServer
+
+int main(int argc, char * argv[])
+{
+	rclcpp::init(argc, argv);
+	auto node = std::make_shared<FibonacciActionServer>();
+	rclcpp::spin(node);
+	rclcpp::shutdown();
+
+	return 0;
+}

prover_rclpy/setup.py

@@ -36,6 +36,7 @@
             'listener = src.listener:main',
             #'rclpy_585 = src.rclpy_585:main',
             #'rclpy_760 = src.rclpy_760:main',
+            'rclcpp_2101_client = src.rclcpp_2101_client:main',
             'rclpy_792 = src.rclpy_792:main',
             'rclpy_client_822 = src.rclpy_client_822:main',
             'rclpy_server_822 = src.rclpy_server_822:main',

prover_rclpy/src/rclcpp_2101_client.py

@@ -0,0 +1,55 @@
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from action_tutorials_interfaces.action import Fibonacci
+
+
+class FibonacciActionClient(Node):
+
+    def __init__(self):
+        super().__init__('fibonacci_action_client')
+        self._action_client = ActionClient(self, Fibonacci, 'fibonacci')
+
+    def send_goal(self, order):
+        goal_msg = Fibonacci.Goal()
+        goal_msg.order = order
+
+        self._action_client.wait_for_server()
+
+        self._send_goal_future = self._action_client.send_goal_async(goal_msg)
+        self._send_goal_future.add_done_callback(self.goal_response_callback)
+
+    def goal_response_callback(self, future):
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().info('Goal rejected :(')
+            return
+
+        self.get_logger().info('Goal accepted :)')
+
+        self.get_logger().info("goal_response_callback: get_result_async")
+        self._get_result_future = goal_handle.get_result_async()
+        self.get_logger().info("goal_response_callback: add_done_callback(result_callback)")
+        self._get_result_future.add_done_callback(self.get_result_callback)
+        self.get_logger().info("goal_response_callback: done")
+
+    def get_result_callback(self, future):
+        result = future.result()
+        self.get_logger().info('Result: status {}'.format(result.status))
+        self.get_logger().info('Result: {0}'.format(result.result.sequence))
+        rclpy.shutdown()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    action_client = FibonacciActionClient()
+
+    action_client.send_goal(10)
+
+    rclpy.spin(action_client)
+
+
+if __name__ == '__main__':
+    main()