Add template classes for ROS2 action clients and servers (#646)

* Add template classes for ROS2 action clients and servers
* Add .action files
* Add a ComputePathToPose action (missed on the first checkin)
* Remove the simple_action_client template (not useful after all) and update action test
* Address some review feedback
* Serialize access to the updated goal handle
* Use preempt terminology in simple_action_server template

nav2_msgs/CMakeLists.txt

@@ -7,21 +7,29 @@ find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
 
 nav2_package()
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"
-  "msg/Path.msg"
   "msg/MissionPlan.msg"
+  "msg/Path.msg"
   "msg/TaskStatus.msg"
   "msg/VoxelGrid.msg"
   "srv/GetCostmap.srv"
   "srv/ClearCostmapExceptRegion.srv"
   "srv/ClearCostmapAroundRobot.srv"
   "srv/ClearEntireCostmap.srv"
-  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs
+  "action/BackUp.action"
+  "action/ComputePathToPose.action"
+  "action/ExecuteMission.action"
+  "action/FollowPath.action"
+  "action/NavigateToPose.action"
+  "action/Spin.action"
+  "action/Stop.action"
+  DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

nav2_msgs/action/BackUp.action

+#goal definition
+geometry_msgs/Point target_location
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/action/ComputePathToPose.action

+#goal definition
+geometry_msgs/PoseStamped pose
+---
+#result definition
+nav2_msgs/Path path
+---
+#feedback

nav2_msgs/action/ExecuteMission.action

+#goal definition
+nav2_msgs/MissionPlan mission_plan
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/action/FollowPath.action

+#goal definition
+nav2_msgs/Path path
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/action/NavigateToPose.action

+#goal definition
+geometry_msgs/PoseStamped pose
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/action/Spin.action

+#goal definition
+geometry_msgs/QuaternionStamped goal
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/action/Stop.action

+#goal definition
+std_msgs/Empty goal
+---
+#result definition
+std_msgs/Empty result
+---
+#feedback

nav2_msgs/package.xml

@@ -17,12 +17,14 @@
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>rosidl_default_generators</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>action_msgs</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>action_msgs</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_util/CMakeLists.txt

@@ -12,6 +12,8 @@ find_package(SDL_image REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(lifecycle_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(test_msgs REQUIRED)
 
 nav2_package()
 
@@ -34,6 +36,8 @@ ament_target_dependencies(${library_name}
   nav_msgs
   geometry_msgs
   lifecycle_msgs
+  rclcpp_action
+  test_msgs
 )
 
 add_library(map_lib SHARED

nav2_util/include/nav2_util/lifecycle_utils.hpp

-// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2019 Intel Corporation
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.

nav2_util/include/nav2_util/simple_action_server.hpp

+// Copyright (c) 2019 Intel Corporation
+//
+// 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.
+
+#ifndef NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
+#define NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+namespace nav2_util
+{
+
+template<typename ActionT>
+class SimpleActionServer
+{
+public:
+  typedef std::function<
+      void (const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)> ExecuteCallback;
+
+  explicit SimpleActionServer(
+    rclcpp::Node::SharedPtr node,
+    const std::string & action_name,
+    ExecuteCallback execute_callback)
+  : node_(node), execute_callback_(execute_callback)
+  {
+    auto handle_goal =
+      [](const rclcpp_action::GoalUUID &, std::shared_ptr<const typename ActionT::Goal>)
+      {
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+      };
+
+    auto handle_cancel = [](std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>)
+      {
+        return rclcpp_action::CancelResponse::ACCEPT;
+      };
+
+    auto handle_accepted = [this](std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle)
+      {
+        std::lock_guard<std::mutex> lock(update_mutex_);
+
+        // If we're currently working on a task, set a flag so that the
+        // action server can grab the pre-empting request in its loop
+        if (received_handle_ != nullptr && received_handle_->is_active()) {
+          preempt_requested_ = true;
+          received_handle_ = handle;
+        } else {
+          // Otherwise, safe to start a new task
+          received_handle_ = handle;
+          std::thread{execute_callback_, handle}.detach();
+        }
+      };
+
+    action_server_ = rclcpp_action::create_server<ActionT>(
+      node_,
+      action_name,
+      handle_goal,
+      handle_cancel,
+      handle_accepted);
+  }
+
+  bool preempt_requested()
+  {
+    std::lock_guard<std::mutex> lock(update_mutex_);
+    return preempt_requested_;
+  }
+
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
+  get_updated_goal_handle()
+  {
+    std::lock_guard<std::mutex> lock(update_mutex_);
+    preempt_requested_ = false;
+    return received_handle_;
+  }
+
+protected:
+  // The SimpleActionServer isn't itself a node, so needs to know which one to use
+  rclcpp::Node::SharedPtr node_;
+
+  ExecuteCallback execute_callback_;
+
+  std::mutex update_mutex_;
+  bool preempt_requested_{false};
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> received_handle_;
+
+  typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
+};
+
+}  // namespace nav2_util
+
+#endif   // NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_

nav2_util/package.xml

@@ -20,6 +20,8 @@
   <build_depend>sdl</build_depend>
   <build_depend>sdl-image</build_depend>
   <build_depend>lifecycle_msgs</build_depend>
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>test_msgs</build_depend>
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclcpp</exec_depend>
@@ -30,6 +32,7 @@
   <exec_depend>sdl</exec_depend>
   <exec_depend>sdl-image</exec_depend>
   <exec_depend>lifecycle_msgs</exec_depend>
+  <build_depend>rclcpp_action</build_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
@@ -39,6 +42,8 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>std_srvs</test_depend>
   <test_depend>rclcpp_lifecycle</test_depend>
+  <test_depend>rclcpp_action</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_util/test/CMakeLists.txt

@@ -16,3 +16,7 @@ find_package(rclcpp_lifecycle REQUIRED)
 ament_add_gtest(test_lifecycle_utils test_lifecycle_utils.cpp)
 ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle)
 target_link_libraries(test_lifecycle_utils ${library_name})
+
+ament_add_gtest(test_actions test_actions.cpp)
+ament_target_dependencies(test_actions rclcpp_action)
+target_link_libraries(test_actions ${library_name})

nav2_util/test/test_actions.cpp

+// Copyright (c) 2019 Intel Corporation
+//
+// 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 <chrono>
+#include <memory>
+#include <thread>
+
+#include "gtest/gtest.h"
+#include "nav2_util/node_utils.hpp"
+#include "nav2_util/simple_action_server.hpp"
+#include "test_msgs/action/fibonacci.hpp"
+
+using Fibonacci = test_msgs::action::Fibonacci;
+using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class FibonacciServerNode : public rclcpp::Node
+{
+public:
+  FibonacciServerNode()
+  : rclcpp::Node("fibonacci_server_node")
+  {
+  }
+
+  ~FibonacciServerNode()
+  {
+  }
+
+  void on_init()
+  {
+    action_server_ = std::make_shared<nav2_util::SimpleActionServer<Fibonacci>>(
+      shared_from_this(),
+      "fibonacci",
+      std::bind(&FibonacciServerNode::execute, this, std::placeholders::_1));
+  }
+
+  void on_term()
+  {
+    action_server_.reset();
+  }
+
+  void execute(const std::shared_ptr<GoalHandle> goal_handle)
+  {
+    // The goal may be pre-empted, so keep a pointer to the current goal
+    std::shared_ptr<GoalHandle> current_goal_handle = goal_handle;
+
+    rclcpp::Rate loop_rate(10);
+
+preempted:
+    // Initialize the goal, feedback, and result
+    auto goal = current_goal_handle->get_goal();
+    auto feedback = std::make_shared<Fibonacci::Feedback>();
+    auto result = std::make_shared<Fibonacci::Result>();
+
+    // Fibonacci-specific initialization
+    auto & sequence = feedback->sequence;
+    sequence.push_back(0);
+    sequence.push_back(1);
+
+    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
+      // Check if this action has been canceled
+      if (current_goal_handle->is_canceling()) {
+        result->sequence = sequence;
+        current_goal_handle->canceled(result);
+        return;
+      }
+
+      // Check if we've gotten an new goal, pre-empting the current one
+      if (action_server_->preempt_requested()) {
+        current_goal_handle = action_server_->get_updated_goal_handle();
+        goto preempted;
+      }
+
+      // Update the sequence
+      sequence.push_back(sequence[i] + sequence[i - 1]);
+
+      // Publish feedback
+      current_goal_handle->publish_feedback(feedback);
+      loop_rate.sleep();
+    }
+
+    // Check if goal is done
+    if (rclcpp::ok()) {
+      result->sequence = sequence;
+      current_goal_handle->succeed(result);
+    }
+  }
+
+private:
+  std::shared_ptr<nav2_util::SimpleActionServer<Fibonacci>> action_server_;
+};
+
+class RclCppFixture
+{
+public:
+  RclCppFixture()
+  {
+    rclcpp::init(0, nullptr);
+    server_thread_ =
+      std::make_unique<std::thread>(std::bind(&RclCppFixture::server_thread_func, this));
+  }
+
+  ~RclCppFixture()
+  {
+    rclcpp::shutdown();
+    server_thread_->join();
+  }
+
+  void server_thread_func()
+  {
+    auto node = std::make_shared<FibonacciServerNode>();
+    node->on_init();
+    rclcpp::spin(node->get_node_base_interface());
+    node->on_term();
+    node.reset();
+  }
+
+  std::unique_ptr<std::thread> server_thread_;
+};
+
+RclCppFixture g_rclcppfixture;
+
+class ActionTestNode : public rclcpp::Node
+{
+public:
+  ActionTestNode()
+  : rclcpp::Node(nav2_util::generate_internal_node_name("action_test_node"))
+  {
+  }
+
+  void on_init()
+  {
+    action_client_ = rclcpp_action::create_client<Fibonacci>(shared_from_this(), "fibonacci");
+    action_client_->wait_for_action_server();
+  }
+
+  void on_term()
+  {
+    action_client_.reset();
+  }
+
+  rclcpp_action::Client<Fibonacci>::SharedPtr action_client_;
+};
+
+class ActionTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    node_ = std::make_shared<ActionTestNode>();
+    node_->on_init();
+  }
+
+  void TearDown() override
+  {
+    node_->on_term();
+    node_.reset();
+  }
+
+  std::shared_ptr<ActionTestNode> node_;
+};
+
+TEST_F(ActionTest, test_simple_action)
+{
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+  goal.order = 12;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal);
+  ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+    future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  ASSERT_EQ(rclcpp::spin_until_future_complete(node_, future_result),
+    rclcpp::executor::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  ASSERT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  ASSERT_EQ(sum, 376);
+}
+
+TEST_F(ActionTest, test_simple_action_with_feedback)
+{
+  int feedback_sum = 0;
+
+  // A callback to accumulate the intermediate values
+  auto feedback_callback = [&feedback_sum](
+    rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr /*goal_handle*/,
+    const std::shared_ptr<const Fibonacci::Feedback> feedback)
+    {
+      feedback_sum += feedback->sequence.back();
+    };
+
+  // The goal for this invocation
+  auto goal = Fibonacci::Goal();
+  goal.order = 10;
+
+  // Send the goal
+  auto future_goal_handle = node_->action_client_->async_send_goal(goal, feedback_callback);
+  ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+    future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+  auto goal_handle = future_goal_handle.get();
+
+  // Wait for the result
+  auto future_result = node_->action_client_->async_get_result(goal_handle);
+  ASSERT_EQ(rclcpp::spin_until_future_complete(node_,
+    future_result), rclcpp::executor::FutureReturnCode::SUCCESS);
+
+  // The final result
+  rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get();
+  ASSERT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED);
+
+  // Sum all of the values in the requested fibonacci series
+  int sum = 0;
+  for (auto number : result.result->sequence) {
+    sum += number;
+  }
+
+  ASSERT_EQ(sum, 143);
+  ASSERT_GE(feedback_sum, 0);  // We should have received *some* feedback
+}

nav2_util/test/test_execution_timer.cpp

@@ -12,9 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "nav2_util/execution_timer.hpp"
 #include <chrono>
 #include <thread>
+
+#include "nav2_util/execution_timer.hpp"
 #include "gtest/gtest.h"
 
 using nav2_util::ExecutionTimer;

nav2_util/test/test_lifecycle_utils.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "nav2_util/lifecycle_utils.hpp"
 #include <thread>
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
+#include <vector>
+
 #include "gtest/gtest.h"
+#include "nav2_util/lifecycle_utils.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 using nav2_util::bringup_lifecycle_nodes;