rclcpp_action Server implementation (ros2#593)

* Commiting to back up work, does not function

* Can call user callback when goal request received

* fini action server in destructor

* rename user callback virtual functions

* handle_execute test passes

* Remove out of date comment

* Refactor execute into three functions

* Remove unused file

* Add failing cancel test

* Cancel test passes

* Remove out of date comments

* Make sure server publishes status when accepting a goal

* Send status when goals transition to cancelling

* Refactored sending goal request to its own function

* Refactor cancel request into it's own function

* Comment with remaining tests

* Executing and terminal state statuses

* publish feedback works

* server sends result to clients that request it

* Remove out of date comment

* Add ServerGoalHandle::is_active()

* Cleanup when goals expire

* Can pass in action server options

* cpplint and uncrustify fixes

* Fix clang warnings

* Copy rcl goal handle

* Fix clang warning

* Use intermediate value to avoid left shift on 32bit integer

* RCLCPP_ACTION_PUBLIC everwhere

* Change callback parameter from C type to C++

* Add accessors for request and uuid

* Feedback must include goal id

* Document Server<> and ServerBase<>

* handle_execute -> handle_accepted

* Test deferred execution

* only publish feedback if goal is executing

* Documentation for ServerGoalHandle

* document msg parameters

* remove unnecessary fini

* notify_goal_done only takes server

* Use unique_indentifier_msgs

* create_server accepts group and removes waitable

* uncrustify

* Use weak ptr to avoid crash if goal handle lives longer than server

* Handle goal callback const message

* Goal handle doesn't have server pointer anymore

* Lock goal_handles_ on Server<>

* rcl_action_server_t protected with mutex

* ServerBase results protected with mutex

* protect rcl goal handle with mutex

* is_cancel_request -> is_canceling

* Add missing include

* use GoalID and change uuid -> goal_id

* Keep rcl goal handle alive until it expires on server

* uncrustify

* Move UUID hash

* Log messages in server

* ACTION -> ActionT

* Cancel abandoned goal handles

* Add convert() for C and C++ goal id

* Remove unused variable

* Constant reference

* Move variable declaration down

* is_ready if goal expired

* map[] default constructs if it doesn't exist

* Use rcl_action_get_goal_status_array()

* Array -> GoalID

* Use reentrant mutex for everything

* comment

* scope exit to fini cancel response

* using GoalID

rclcpp_action/CMakeLists.txt

@@ -22,6 +22,7 @@ set(${PROJECT_NAME}_SRCS
   src/client.cpp
   src/server.cpp
   src/server_goal_handle.cpp
+  src/types.cpp
 )
 
 add_library(${PROJECT_NAME}

rclcpp_action/include/rclcpp_action/create_server.hpp

@@ -15,7 +15,13 @@
 #ifndef RCLCPP_ACTION__CREATE_SERVER_HPP_
 #define RCLCPP_ACTION__CREATE_SERVER_HPP_
 
+#include <rcl_action/action_server.h>
+
 #include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_clock_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <rclcpp/node_interfaces/node_waitables_interface.hpp>
 
 #include <memory>
 #include <string>
@@ -25,19 +31,59 @@
 
 namespace rclcpp_action
 {
-template<typename ACTION>
-typename Server<ACTION>::SharedPtr
+template<typename ActionT>
+typename Server<ActionT>::SharedPtr
 create_server(
-  rclcpp::Node * node,
+  rclcpp::Node::SharedPtr node,
   const std::string & name,
-  typename Server<ACTION>::GoalCallback handle_goal,
-  typename Server<ACTION>::CancelCallback handle_cancel)
+  typename Server<ActionT>::GoalCallback handle_goal,
+  typename Server<ActionT>::CancelCallback handle_cancel,
+  typename Server<ActionT>::AcceptedCallback handle_accepted,
+  const rcl_action_server_options_t & options = rcl_action_server_get_default_options(),
+  rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr)
 {
-  return Server<ACTION>::make_shared(
-    node->get_node_base_interface(),
-    name,
-    handle_goal,
-    handle_cancel);
+  std::weak_ptr<rclcpp::node_interfaces::NodeWaitablesInterface> weak_node =
+    node->get_node_waitables_interface();
+  std::weak_ptr<rclcpp::callback_group::CallbackGroup> weak_group = group;
+  bool group_is_null = (nullptr == group.get());
+
+  auto deleter = [weak_node, weak_group, group_is_null](Server<ActionT> * ptr)
+    {
+      if (nullptr == ptr) {
+        return;
+      }
+      auto shared_node = weak_node.lock();
+      if (!shared_node) {
+        return;
+      }
+      // API expects a shared pointer, give it one with a deleter that does nothing.
+      std::shared_ptr<Server<ActionT>> fake_shared_ptr(ptr, [](Server<ActionT> *) {});
+
+      if (group_is_null) {
+        // Was added to default group
+        shared_node->remove_waitable(fake_shared_ptr, nullptr);
+      } else {
+        // Was added to a specfic group
+        auto shared_group = weak_group.lock();
+        if (shared_group) {
+          shared_node->remove_waitable(fake_shared_ptr, shared_group);
+        }
+      }
+      delete ptr;
+    };
+
+  std::shared_ptr<Server<ActionT>> action_server(new Server<ActionT>(
+      node->get_node_base_interface(),
+      node->get_node_clock_interface(),
+      node->get_node_logging_interface(),
+      name,
+      options,
+      handle_goal,
+      handle_cancel,
+      handle_accepted), deleter);
+
+  node->get_node_waitables_interface()->add_waitable(action_server, group);
+  return action_server;
 }
 }  // namespace rclcpp_action
 #endif  // RCLCPP_ACTION__CREATE_SERVER_HPP_