Add rclcpp action examples with member functions

rclcpp/minimal_action_client/CMakeLists.txt

@@ -15,6 +15,12 @@ find_package(example_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 
+add_executable(action_client_member_functions member_functions.cpp)
+ament_target_dependencies(action_client_member_functions
+  "rclcpp"
+  "rclcpp_action"
+  "example_interfaces")
+
 add_executable(action_client_not_composable not_composable.cpp)
 ament_target_dependencies(action_client_not_composable
   "rclcpp"
@@ -34,6 +40,7 @@ ament_target_dependencies(action_client_not_composable_with_feedback
   "example_interfaces")
 
 install(TARGETS
+  action_client_member_functions
   action_client_not_composable
   action_client_not_composable_with_cancel
   action_client_not_composable_with_feedback

rclcpp/minimal_action_client/member_functions.cpp

@@ -0,0 +1,143 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <inttypes.h>
+#include <memory>
+#include <string>
+#include <iostream>
+#include "example_interfaces/action/fibonacci.hpp"
+#include "rclcpp/rclcpp.hpp"
+// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class MinimalActionClient : public rclcpp::Node
+{
+public:
+  using Fibonacci = example_interfaces::action::Fibonacci;
+  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
+
+  MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
+  : Node("minimal_action_client", node_options), goal_done_(false)
+  {
+    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
+      this->get_node_base_interface(),
+      this->get_node_graph_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      "fibonacci");
+
+    this->timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(500),
+      std::bind(&MinimalActionClient::send_goal, this));
+  }
+
+  bool is_goal_done() const
+  {
+    return this->goal_done_;
+  }
+
+  void send_goal()
+  {
+    using namespace std::placeholders;
+
+    this->timer_->cancel();
+
+    this->goal_done_ = false;
+
+    if (!this->client_ptr_) {
+      RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
+    }
+
+    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
+      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
+      this->goal_done_ = true;
+      return;
+    }
+
+    auto goal_msg = Fibonacci::Goal();
+    goal_msg.order = 10;
+
+    RCLCPP_INFO(this->get_logger(), "Sending goal");
+
+    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
+    send_goal_options.goal_response_callback =
+      std::bind(&MinimalActionClient::goal_response_callback, this, _1);
+    send_goal_options.feedback_callback =
+      std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
+    send_goal_options.result_callback =
+      std::bind(&MinimalActionClient::result_callback, this, _1);
+    auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
+  }
+
+private:
+  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  bool goal_done_;
+
+  void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
+  {
+    auto goal_handle = future.get();
+    if (!goal_handle) {
+      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
+    } else {
+      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
+    }
+  }
+
+  void feedback_callback(
+    GoalHandleFibonacci::SharedPtr,
+    const std::shared_ptr<const Fibonacci::Feedback> feedback)
+  {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Next number in sequence received: %" PRId64,
+      feedback->sequence.back());
+  }
+
+  void result_callback(const GoalHandleFibonacci::WrappedResult & result)
+  {
+    this->goal_done_ = true;
+    switch(result.code) {
+      case rclcpp_action::ResultCode::SUCCEEDED:
+        break;
+      case rclcpp_action::ResultCode::ABORTED:
+        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+        return;
+      case rclcpp_action::ResultCode::CANCELED:
+        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+        return;
+      default:
+        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+        return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Result received");
+    for (auto number : result.result->sequence) {
+      RCLCPP_INFO(this->get_logger(), "%" PRId64, number);
+    }
+  }
+};  // class MinimalActionClient
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto action_client = std::make_shared<MinimalActionClient>();
+
+  while (!action_client->is_goal_done()) {
+    rclcpp::spin_some(action_client);
+  }
+
+  rclcpp::shutdown();
+  return 0;
+}

rclcpp/minimal_action_client/not_composable_with_feedback.cpp

@@ -50,7 +50,9 @@ int main(int argc, char ** argv)
 
   RCLCPP_INFO(g_node->get_logger(), "Sending goal");
   // Ask server to achieve some goal and wait until it's accepted
-  auto goal_handle_future = action_client->async_send_goal(goal_msg, feedback_callback);
+  auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
+  send_goal_options.feedback_callback = feedback_callback;
+  auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_options);
 
   if (rclcpp::spin_until_future_complete(g_node, goal_handle_future) !=
     rclcpp::executor::FutureReturnCode::SUCCESS)

rclcpp/minimal_action_server/CMakeLists.txt

@@ -15,13 +15,21 @@ find_package(example_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 
+add_executable(action_server_member_functions member_functions.cpp)
+ament_target_dependencies(action_server_member_functions
+  "rclcpp"
+  "rclcpp_action"
+  "example_interfaces")
+
 add_executable(action_server_not_composable not_composable.cpp)
 ament_target_dependencies(action_server_not_composable
   "rclcpp"
   "rclcpp_action"
   "example_interfaces")
 
-install(TARGETS action_server_not_composable
+install(TARGETS
+  action_server_not_composable
+  action_server_member_functions
   DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

rclcpp/minimal_action_server/member_functions.cpp

@@ -0,0 +1,125 @@
+// Copyright 2019 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <inttypes.h>
+#include <memory>
+#include "example_interfaces/action/fibonacci.hpp"
+#include "rclcpp/rclcpp.hpp"
+// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class MinimalActionServer : public rclcpp::Node
+{
+public:
+  using Fibonacci = example_interfaces::action::Fibonacci;
+  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+  MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+    : Node("minimal_action_server", options)
+  {
+    using namespace std::placeholders;
+
+    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
+      this->get_node_base_interface(),
+      this->get_node_clock_interface(),
+      this->get_node_logging_interface(),
+      this->get_node_waitables_interface(),
+      "fibonacci",
+      std::bind(&MinimalActionServer::handle_goal, this, _1, _2),
+      std::bind(&MinimalActionServer::handle_cancel, this, _1),
+      std::bind(&MinimalActionServer::handle_accepted, this, _1));
+  }
+
+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;
+    // Let's reject sequences that are over 9000
+    if (goal->order > 9000)
+    {
+      return rclcpp_action::GoalResponse::REJECT;
+    }
+    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 execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
+  {
+    RCLCPP_INFO(this->get_logger(), "Executing goal");
+    rclcpp::Rate loop_rate(1);
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+    auto& sequence = feedback->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 (goal_handle->is_canceling())
+      {
+        result->sequence = sequence;
+        goal_handle->canceled(result);
+        RCLCPP_INFO(this->get_logger(), "Goal Canceled");
+        return;
+      }
+      // Update sequence
+      sequence.push_back(sequence[i] + sequence[i - 1]);
+      // Publish feedback
+      goal_handle->publish_feedback(feedback);
+      RCLCPP_INFO(this->get_logger(), "Publish Feedback");
+
+      loop_rate.sleep();
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->sequence = sequence;
+      goal_handle->succeed(result);
+      RCLCPP_INFO(this->get_logger(), "Goal Suceeded");
+    }
+  }
+
+  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(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
+  }
+};  // class MinimalActionServer
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto action_server = std::make_shared<MinimalActionServer>();
+
+  rclcpp::spin(action_server);
+
+  rclcpp::shutdown();
+  return 0;
+}

rclcpp/minimal_action_server/not_composable.cpp

@@ -23,7 +23,7 @@ using Fibonacci = example_interfaces::action::Fibonacci;
 using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
 
 rclcpp_action::GoalResponse handle_goal(
-  const std::array<uint8_t, 16> & uuid, std::shared_ptr<const Fibonacci::Goal> goal)
+  const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal)
 {
   RCLCPP_INFO(rclcpp::get_logger("server"), "Got goal request with order %d", goal->order);
   (void)uuid;