From 1b5168195bf70f908a5a2a3df5f974ec96605b36 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 2 Dec 2016 01:05:59 -0800 Subject: [PATCH] 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 | 29 +++- rclcpp/include/rclcpp/node.hpp | 5 + rclcpp/include/rclcpp/node_impl.hpp | 16 +-- rclcpp/include/rclcpp/service.hpp | 53 +++++-- rclcpp/src/rclcpp/node.cpp | 27 ++++ rclcpp/src/rclcpp/service.cpp | 8 +- rclcpp/test/mock_msgs/srv/Mock.srv | 3 + .../test/test_externally_defined_services.cpp | 135 ++++++++++++++++++ 8 files changed, 248 insertions(+), 28 deletions(-) create mode 100644 rclcpp/test/mock_msgs/srv/Mock.srv create mode 100644 rclcpp/test/test_externally_defined_services.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index e7ce2e6..5d68977 100644 --- a/rclcpp/CMakeLists.txt +++ b/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( diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 003391a..2d578b2 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/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) diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 2141096..f80da7d 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/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(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(¬ify_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; } diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index d8719a7..db224f3 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -44,7 +44,11 @@ public: RCLCPP_PUBLIC ServiceBase( std::shared_ptr node_handle, - const std::string service_name); + const std::string & service_name); + + RCLCPP_PUBLIC + explicit ServiceBase( + std::shared_ptr node_handle); RCLCPP_PUBLIC virtual ~ServiceBase(); @@ -68,8 +72,9 @@ protected: std::shared_ptr 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(); + // 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 node_handle, + rcl_service_t * service_handle, + AnyServiceCallback 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(); + } } } diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 29810a8..604aae6 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/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(¬ify_guard_condition_) != RCL_RET_OK) { + throw std::runtime_error( + std::string( + "Failed to notify waitset on service creation: ") + rmw_get_error_string()); + } +} diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 24d0ad6..136d688 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -29,10 +29,14 @@ using rclcpp::service::ServiceBase; ServiceBase::ServiceBase( std::shared_ptr 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 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_; } diff --git a/rclcpp/test/mock_msgs/srv/Mock.srv b/rclcpp/test/mock_msgs/srv/Mock.srv new file mode 100644 index 0000000..06a8369 --- /dev/null +++ b/rclcpp/test/mock_msgs/srv/Mock.srv @@ -0,0 +1,3 @@ +bool request +--- +bool response diff --git a/rclcpp/test/test_externally_defined_services.cpp b/rclcpp/test/test_externally_defined_services.cpp new file mode 100644 index 0000000..03161a2 --- /dev/null +++ b/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 + +#include +#include + +#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/*req*/, + std::shared_ptr/*resp*/) +{} + +TEST_F(TestExternallyDefinedServices, default_behavior) { + auto node_handle = rclcpp::node::Node::make_shared("base_node"); + + try { + auto srv = node_handle->create_service("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 cb; + + // don't initialize the service + // expect fail + try { + rclcpp::service::Service(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 cb; + + try { + rclcpp::service::Service(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 cb; + + { + // Call constructor + rclcpp::service::Service srv_cpp(node_handle->get_shared_node_handle(), + &service_handle, cb); + // Call destructor + } + + if (!service_handle.impl) { + FAIL(); + return; + } + SUCCEED(); +}