construct service from existing rcl_service_t (#279)

* (dev) construct service from existing rcl_service_t

* (refactor) extract method for adding a service to a node

* (fix) stop mock msgs from being installed

* service takes rcl_node_t*

* correct typo

* add_service has to be public

* uncrustify

* correctly initialize service_handle

* (fix) address review comments

* (fix) pass shared pointer by value

* (fix) return to shared node handle pointer

* (fix) make find_package(rmw) required

* style

* (revert) leave c++11 flags within CXX flags

* (fix) unused variable warning

* (fix) remove unnecessary if in cmake

rclcpp/CMakeLists.txt

@@ -11,7 +11,8 @@ find_package(rmw_implementation_cmake REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-std=c++11 -Wall -Wextra -Wpedantic)
+  set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-std=c++11")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
 include_directories(include)
@@ -127,6 +128,32 @@ if(BUILD_TESTING)
     )
     target_link_libraries(test_find_weak_nodes ${PROJECT_NAME})
   endif()
+
+  get_default_rmw_implementation(default_rmw)
+  find_package(${default_rmw} REQUIRED)
+  get_rmw_typesupport(typesupport_impls_cpp "${default_rmw}" LANGUAGE "cpp")
+  get_rmw_typesupport(typesupport_impls_c "${default_rmw}" LANGUAGE "c")
+  set(mock_msg_files
+    "test/mock_msgs/srv/Mock.srv")
+  rosidl_generate_interfaces(mock_msgs
+    ${mock_msg_files}
+    SKIP_INSTALL)
+
+  ament_add_gtest(test_externally_defined_services test/test_externally_defined_services.cpp)
+  if(TARGET test_externally_defined_services)
+    target_include_directories(test_externally_defined_services PUBLIC
+      ${rcl_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_externally_defined_services ${PROJECT_NAME})
+    foreach(typesupport_impl_cpp ${typesupport_impls_cpp})
+      rosidl_target_interfaces(test_externally_defined_services
+        mock_msgs ${typesupport_impl_cpp})
+    endforeach()
+    foreach(typesupport_impl_c ${typesupport_impls_c})
+      rosidl_target_interfaces(test_externally_defined_services
+        mock_msgs ${typesupport_impl_c})
+    endforeach()
+  endif()
 endif()
 
 ament_package(

rclcpp/include/rclcpp/node.hpp

@@ -379,6 +379,11 @@ public:
 
   std::atomic_bool has_executor;
 
+  RCLCPP_PUBLIC
+  void
+  add_service(rclcpp::service::ServiceBase::SharedPtr service,
+    rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr);
+
 private:
   RCLCPP_DISABLE_COPY(Node)
 

rclcpp/include/rclcpp/node_impl.hpp

@@ -344,21 +344,7 @@ Node::create_service(
     node_handle_,
     service_name, any_service_callback, service_options);
   auto serv_base_ptr = std::dynamic_pointer_cast<service::ServiceBase>(serv);
-  if (group) {
-    if (!group_in_node(group)) {
-      // TODO(jacquelinekay): use custom exception
-      throw std::runtime_error("Cannot create service, group not in node.");
-    }
-    group->add_service(serv_base_ptr);
-  } else {
-    default_callback_group_->add_service(serv_base_ptr);
-  }
-  number_of_services_++;
-  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
-    throw std::runtime_error(
-            std::string(
-              "Failed to notify waitset on service creation: ") + rmw_get_error_string());
-  }
+  add_service(serv_base_ptr, group);
   return serv;
 }
 

rclcpp/include/rclcpp/service.hpp

@@ -44,7 +44,11 @@ public:
   RCLCPP_PUBLIC
   ServiceBase(
     std::shared_ptr<rcl_node_t> node_handle,
-    const std::string service_name);
+    const std::string & service_name);
+
+  RCLCPP_PUBLIC
+  explicit ServiceBase(
+    std::shared_ptr<rcl_node_t> node_handle);
 
   RCLCPP_PUBLIC
   virtual ~ServiceBase();
@@ -68,8 +72,9 @@ protected:
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  rcl_service_t service_handle_ = rcl_get_zero_initialized_service();
+  rcl_service_t * service_handle_ = nullptr;
   std::string service_name_;
+  bool owns_rcl_handle_ = true;
 };
 
 using any_service_callback::AnyServiceCallback;
@@ -100,25 +105,53 @@ public:
     using rosidl_generator_cpp::get_service_type_support_handle;
     auto service_type_support_handle = get_service_type_support_handle<ServiceT>();
 
+    // rcl does the static memory allocation here
+    service_handle_ = new rcl_service_t;
+    *service_handle_ = rcl_get_zero_initialized_service();
+
     if (rcl_service_init(
-        &service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
+        service_handle_, node_handle.get(), service_type_support_handle, service_name.c_str(),
         &service_options) != RCL_RET_OK)
     {
-      throw std::runtime_error(
-              std::string("could not create service: ") +
+      throw std::runtime_error(std::string("could not create service: ") +
               rcl_get_error_string_safe());
     }
   }
 
+  Service(
+    std::shared_ptr<rcl_node_t> node_handle,
+    rcl_service_t * service_handle,
+    AnyServiceCallback<ServiceT> any_callback)
+  : ServiceBase(node_handle),
+    any_callback_(any_callback)
+  {
+    // check if service handle was initialized
+    // TODO(karsten1987): Take this verification
+    // directly in rcl_*_t
+    // see: https://github.com/ros2/rcl/issues/81
+    if (!service_handle->impl) {
+      // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here)
+      throw std::runtime_error(
+        std::string("rcl_service_t in constructor argument must be initialized beforehand."));
+      // *INDENT-ON*
+    }
+    service_handle_ = service_handle;
+    service_name_ = std::string(rcl_service_get_service_name(service_handle));
+    owns_rcl_handle_ = false;
+  }
+
   Service() = delete;
 
   virtual ~Service()
   {
-    if (rcl_service_fini(&service_handle_, node_handle_.get()) != RCL_RET_OK) {
-      std::stringstream ss;
-      ss << "Error in destruction of rcl service_handle_ handle: " <<
-        rcl_get_error_string_safe() << '\n';
-      (std::cerr << ss.str()).flush();
+    // check if you have ownership of the handle
+    if (owns_rcl_handle_) {
+      if (rcl_service_fini(service_handle_, node_handle_.get()) != RCL_RET_OK) {
+        std::stringstream ss;
+        ss << "Error in destruction of rcl service_handle_ handle: " <<
+          rcl_get_error_string_safe() << '\n';
+        (std::cerr << ss.str()).flush();
+      }
     }
   }
 

rclcpp/src/rclcpp/node.cpp

@@ -542,3 +542,30 @@ Node::count_graph_users()
 {
   return graph_users_count_.load();
 }
+
+
+// PROTECTED IMPLEMENTATION
+void
+Node::add_service(rclcpp::service::ServiceBase::SharedPtr serv_base_ptr,
+  rclcpp::callback_group::CallbackGroup::SharedPtr group)
+{
+  if (!serv_base_ptr) {
+    throw std::runtime_error("Cannot add empty service to group");
+  }
+
+  if (group) {
+    if (!group_in_node(group)) {
+      // TODO(jacquelinekay): use custom exception
+      throw std::runtime_error("Cannot create service, group not in node.");
+    }
+    group->add_service(serv_base_ptr);
+  } else {
+    default_callback_group_->add_service(serv_base_ptr);
+  }
+  number_of_services_++;
+  if (rcl_trigger_guard_condition(&notify_guard_condition_) != RCL_RET_OK) {
+    throw std::runtime_error(
+            std::string(
+              "Failed to notify waitset on service creation: ") + rmw_get_error_string());
+  }
+}

rclcpp/src/rclcpp/service.cpp

@@ -29,10 +29,14 @@ using rclcpp::service::ServiceBase;
 
 ServiceBase::ServiceBase(
   std::shared_ptr<rcl_node_t> node_handle,
-  const std::string service_name)
+  const std::string & service_name)
 : node_handle_(node_handle), service_name_(service_name)
 {}
 
+ServiceBase::ServiceBase(std::shared_ptr<rcl_node_t> node_handle)
+: node_handle_(node_handle)
+{}
+
 ServiceBase::~ServiceBase()
 {}
 
@@ -45,5 +49,5 @@ ServiceBase::get_service_name()
 const rcl_service_t *
 ServiceBase::get_service_handle()
 {
-  return &service_handle_;
+  return service_handle_;
 }

rclcpp/test/mock_msgs/srv/Mock.srv

@@ -0,0 +1,3 @@
+bool request
+---
+bool response

rclcpp/test/test_externally_defined_services.cpp

@@ -0,0 +1,135 @@
+// Copyright 2016 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 <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/any_service_callback.hpp"
+#include "rclcpp/service.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rcl/service.h"
+
+#include "rclcpp/srv/mock.hpp"
+#include "rclcpp/srv/mock.h"
+
+class TestExternallyDefinedServices : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+    rclcpp::init(0, nullptr);
+  }
+};
+
+void
+callback(const std::shared_ptr<rclcpp::srv::Mock::Request>/*req*/,
+  std::shared_ptr<rclcpp::srv::Mock::Response>/*resp*/)
+{}
+
+TEST_F(TestExternallyDefinedServices, default_behavior) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  try {
+    auto srv = node_handle->create_service<rclcpp::srv::Mock>("test",
+        callback);
+  } catch (const std::exception &) {
+    FAIL();
+    return;
+  }
+  SUCCEED();
+}
+
+
+TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  // don't initialize the service
+  // expect fail
+  try {
+    rclcpp::service::Service<rclcpp::srv::Mock>(node_handle->get_shared_node_handle(),
+      &service_handle, cb);
+  } catch (const std::runtime_error &) {
+    SUCCEED();
+    return;
+  }
+
+  FAIL();
+}
+
+TEST_F(TestExternallyDefinedServices, extern_defined_initialized) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
+    rclcpp, srv, Mock);
+  if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(),
+    ts, "base_node_service", &service_options) != RCL_RET_OK)
+  {
+    FAIL();
+    return;
+  }
+
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  try {
+    rclcpp::service::Service<rclcpp::srv::Mock>(node_handle->get_shared_node_handle(),
+      &service_handle, cb);
+  } catch (const std::runtime_error &) {
+    FAIL();
+    return;
+  }
+
+  SUCCEED();
+}
+
+TEST_F(TestExternallyDefinedServices, extern_defined_destructor) {
+  auto node_handle = rclcpp::node::Node::make_shared("base_node");
+
+  // mock for externally defined service
+  rcl_service_t service_handle = rcl_get_zero_initialized_service();
+  rcl_service_options_t service_options = rcl_service_get_default_options();
+  const rosidl_service_type_support_t * ts = ROSIDL_GET_TYPE_SUPPORT(
+    rclcpp, srv, Mock);
+  if (rcl_service_init(&service_handle, node_handle->get_rcl_node_handle(),
+    ts, "base_node_service", &service_options) != RCL_RET_OK)
+  {
+    FAIL();
+    return;
+  }
+  rclcpp::any_service_callback::AnyServiceCallback<rclcpp::srv::Mock> cb;
+
+  {
+    // Call constructor
+    rclcpp::service::Service<rclcpp::srv::Mock> srv_cpp(node_handle->get_shared_node_handle(),
+      &service_handle, cb);
+    // Call destructor
+  }
+
+  if (!service_handle.impl) {
+    FAIL();
+    return;
+  }
+  SUCCEED();
+}