Executing action server but nothing is ready

  ros2/rclcpp#2226

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

prover_rclcpp/CMakeLists.txt

@@ -32,6 +32,7 @@ set (CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} "$ENV{AMENT_PREFIX_PATH}")
 find_package(ament_cmake REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcutils)
@@ -40,17 +41,20 @@ find_package(std_srvs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(prover_interfaces REQUIRED)
 find_package(example_interfaces REQUIRED)
+find_package(action_tutorials_interfaces REQUIRED)
 
 function(custom_executable target)
   add_executable(${target} src/${target}.cpp)
   ament_target_dependencies(${target}
     "rclcpp"
+    "rclcpp_action"
     "std_srvs"
     "std_msgs"
     "prover_interfaces"
     "lifecycle_msgs"
     "rclcpp_lifecycle"
-    "example_interfaces")
+    "example_interfaces"
+    "action_tutorials_interfaces")
   target_compile_options(${target}
     PRIVATE "-Wall" "-Wextra" "-Werror" "-Wpedantic" "-pthread")
   target_link_options(${target}
@@ -98,6 +102,7 @@ custom_executable(rclcpp_2138)
 custom_executable(rclcpp_2144)
 custom_executable(rclcpp_2146)
 custom_executable(rclcpp_2166)
+custom_executable(rclcpp_2226)
 custom_executable(rclpy_881)
 
 custom_executable(rmw_fastrtps_554)

prover_rclcpp/package.xml

@@ -15,6 +15,7 @@
 
   <build_depend>lifecycle_msgs</build_depend>
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_action</build_depend>
   <build_depend>rclcpp_components</build_depend>
   <build_depend>rclcpp_lifecycle</build_depend>
   <build_depend>rcutils</build_depend>
@@ -24,9 +25,11 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>prover_interfaces</build_depend>
   <build_depend>example_interfaces</build_depend>
+  <build_depend>action_tutorials_interfaces</build_depend>
 
   <exec_depend>lifecycle_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>rclcpp_lifecycle</exec_depend>
   <exec_depend>rcutils</exec_depend>
@@ -35,6 +38,7 @@
   <exec_depend>std_srvs</exec_depend>
   <exec_depend>prover_interfaces</exec_depend>
   <exec_depend>example_interfaces</exec_depend>
+  <exec_depend>action_tutorials_interfaces</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

prover_rclcpp/src/rclcpp_2226.cpp

@@ -0,0 +1,104 @@
+#include "action_tutorials_interfaces/action/fibonacci.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
+using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+class Server final : public rclcpp::Node {
+public:
+  Server() : rclcpp::Node("action_server") {
+    using namespace std::placeholders;
+
+    m_cb = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+    m_server = rclcpp_action::create_server<Fibonacci>(
+      this,
+      "/test/server",
+      std::bind(&Server::goalHandle, this, _1, _2),
+      std::bind(&Server::handleCancel, this, _1),
+      std::bind(&Server::handleAccepted, this, _1),
+      rcl_action_server_get_default_options(),
+      m_cb
+      );
+  }
+
+  Server(Server const &) = delete;
+  Server(Server &&) noexcept = delete;
+  Server& operator=(Server const &) = delete;
+  Server& operator=(Server &&) noexcept = delete;
+  ~Server() noexcept = default;
+
+  rclcpp::CallbackGroup::SharedPtr m_cb;
+  rclcpp_action::Server<Fibonacci>::SharedPtr m_server;
+
+private:
+  rclcpp_action::GoalResponse goalHandle(rclcpp_action::GoalUUID const &uuid,
+                                         Fibonacci::Goal::ConstSharedPtr goal) {
+    (void)uuid;
+    RCLCPP_INFO_STREAM(get_logger(), "Goal handle");
+    if (goal->order > 100) {
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+  }
+
+  rclcpp_action::CancelResponse handleCancel(std::shared_ptr<GoalHandle> const goalHandle) {
+    RCLCPP_INFO_STREAM(get_logger(), "Handle cancel");
+    (void)goalHandle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+  }
+
+  void handleAccepted(std::shared_ptr<GoalHandle> const goalHandle) {
+    RCLCPP_INFO_STREAM(get_logger(), "Handle accepted");
+    std::thread([goal = goalHandle, this] () -> void {
+                  this->execute(std::move(goal));
+                }).detach();
+  }
+
+  void execute(std::shared_ptr<GoalHandle> const goalHandle) {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    rclcpp::Rate loop_rate(1);
+    const auto goal = goalHandle->get_goal();
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+    auto & sequence = feedback->partial_sequence;
+    sequence.push_back(0);
+    sequence.push_back(1);
+    auto result = std::make_shared<Fibonacci::Result>();
+
+    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+      // Check if there is a cancel request
+      if (goalHandle->is_canceling()) {
+        result->sequence = sequence;
+        goalHandle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal canceled");
+        return;
+      }
+      // Update sequence
+      sequence.push_back(sequence[i] + sequence[i - 1]);
+      // Publish feedback
+      goalHandle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "Publish feedback");
+
+      loop_rate.sleep();
+    }
+
+    if (rclcpp::ok()) {
+      result->sequence = sequence;
+      goalHandle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+    }
+  }
+};
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+
+  auto const server = std::make_shared<Server>();
+
+  rclcpp::executors::MultiThreadedExecutor executor;
+  executor.add_node(server);
+  executor.spin();
+
+  rclcpp::shutdown();
+};