diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index bfe4431..82023e0 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -22,6 +22,7 @@ #include #include +#include namespace rclcpp { @@ -62,9 +63,16 @@ private: timer_ptrs_.push_back(timer_ptr); } + void + add_service(const service::ServiceBase::SharedPtr &service_ptr) + { + service_ptrs_.push_back(service_ptr); + } + CallbackGroupType type_; std::vector subscription_ptrs_; std::vector timer_ptrs_; + std::vector service_ptrs_; std::atomic_bool can_be_taken_from_; }; diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp new file mode 100644 index 0000000..6bda230 --- /dev/null +++ b/rclcpp/include/rclcpp/client.hpp @@ -0,0 +1,65 @@ +/* Copyright 2014 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. + */ + +#ifndef RCLCPP_RCLCPP_CLIENT_HPP_ +#define RCLCPP_RCLCPP_CLIENT_HPP_ + +#include + +#include +#include + +#include + +namespace rclcpp +{ + +// Forward declaration for friend statement +namespace node {class Node;} + +namespace client +{ + +template +class Client +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(Client); + + Client(ros_middleware_interface::ClientHandle client_handle, + std::string& service_name) + : client_handle_(client_handle), service_name_(service_name) + {} + + std::shared_ptr + send_request(std::shared_ptr &req) + { + ::ros_middleware_interface::send_request(client_handle_, req.get()); + + std::shared_ptr res = std::make_shared(); + ::ros_middleware_interface::receive_response(client_handle_, res.get()); + return res; + } + +private: + ros_middleware_interface::ClientHandle client_handle_; + std::string service_name_; + +}; + +} /* namespace client */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 2d14094..8de14cf 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -106,6 +106,7 @@ protected: // Either the subscription or the timer will be set, but not both rclcpp::subscription::SubscriptionBase::SharedPtr subscription; rclcpp::timer::TimerBase::SharedPtr timer; + rclcpp::service::ServiceBase::SharedPtr service; // These are used to keep the scope on the containing items rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; rclcpp::node::Node::SharedPtr node; @@ -126,6 +127,10 @@ protected: { execute_subscription(any_exec->subscription); } + if (any_exec->service) + { + execute_service(any_exec->service); + } // Reset the callback_group, regardless of type any_exec->callback_group->can_be_taken_from_.store(true); // Wake the wait, because it may need to be recalculated or work that @@ -161,6 +166,26 @@ protected: timer->callback_(); } + static void + execute_service( + rclcpp::service::ServiceBase::SharedPtr &service) + { + std::shared_ptr request = service->create_request(); + bool taken = ros_middleware_interface::take_request( + service->service_handle_, + request.get()); + if (taken) + { + service->handle_request(request); + } + else + { + std::cout << "[rclcpp::error] take failed for service on service: " + << service->get_service_name() + << std::endl; + } + } + /*** Populate class storage from stored weak node pointers and wait. ***/ void @@ -170,6 +195,7 @@ protected: bool has_invalid_weak_nodes = false; std::vector subs; std::vector timers; + std::vector services; for (auto &weak_node : weak_nodes_) { auto node = weak_node.lock(); @@ -193,6 +219,10 @@ protected: { timers.push_back(timer); } + for (auto &service : group->service_ptrs_) + { + services.push_back(service); + } } } // Clean up any invalid nodes, if they were detected @@ -224,6 +254,28 @@ protected: subscription->subscription_handle_.data_; subscriber_handle_index += 1; } + + // Use the number of services to allocate memory in the handles + size_t number_of_services = services.size(); + ros_middleware_interface::ServiceHandles service_handles; + service_handles.service_count_ = number_of_services; + // TODO: Avoid redundant malloc's + service_handles.services_ = static_cast( + std::malloc(sizeof(void *) * number_of_services)); + if (service_handles.services_ == NULL) + { + // TODO: Use a different error here? maybe std::bad_alloc? + throw std::runtime_error("Could not malloc for service pointers."); + } + // Then fill the ServiceHandles with ready services + size_t service_handle_index = 0; + for (auto &service : services) + { + service_handles.services_[service_handle_index] = \ + service->service_handle_.data_; + service_handle_index += 1; + } + // Use the number of guard conditions to allocate memory in the handles // Add 2 to the number for the ctrl-c guard cond and the executor's size_t start_of_timer_guard_conds = 2; @@ -254,9 +306,11 @@ protected: timer->guard_condition_.data_; guard_cond_handle_index += 1; } + // Now wait on the waitable subscriptions and timers ros_middleware_interface::wait(subscriber_handles, guard_condition_handles, + service_handles, nonblocking); // If ctrl-c guard condition, return directly if (guard_condition_handles.guard_conditions_[0] != 0) @@ -264,6 +318,7 @@ protected: // Make sure to free memory // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed std::free(subscriber_handles.subscribers_); + std::free(service_handles.services_); std::free(guard_condition_handles.guard_conditions_); return; } @@ -286,9 +341,20 @@ protected: guard_condition_handles_.push_back(handle); } } + // Then the services + for (size_t i = 0; i < number_of_services; ++i) + { + void *handle = service_handles.services_[i]; + if (handle) + { + service_handles_.push_back(handle); + } + } + // Make sure to free memory // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed std::free(subscriber_handles.subscribers_); + std::free(service_handles.services_); std::free(guard_condition_handles.guard_conditions_); } @@ -352,6 +418,35 @@ protected: return rclcpp::timer::TimerBase::SharedPtr(); } + rclcpp::service::ServiceBase::SharedPtr + get_service_by_handle(void * service_handle) + { + for (auto weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + if (!group) + { + continue; + } + for (auto service : group->service_ptrs_) + { + if (service->service_handle_.data_ == service_handle) + { + return service; + } + } + } + } + return rclcpp::service::ServiceBase::SharedPtr(); + } + void remove_subscriber_handle_from_subscriber_handles(void *handle) { @@ -364,6 +459,12 @@ protected: guard_condition_handles_.remove(handle); } + void + remove_service_handle_from_service_handles(void *handle) + { + service_handles_.remove(handle); + } + rclcpp::node::Node::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr &group) { @@ -516,6 +617,67 @@ protected: } } + rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_service( + rclcpp::service::ServiceBase::SharedPtr &service) + { + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto &weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + for (auto &serv : group->service_ptrs_) + { + if (serv == service) + { + return group; + } + } + } + } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); + } + + void + get_next_service(std::shared_ptr &any_exec) + { + for (auto handle : service_handles_) + { + auto service = get_service_by_handle(handle); + if (service) + { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_service(service); + if (!group) + { + // Group was not found, meaning the service is not valid... + // Remove it from the ready list and continue looking + remove_service_handle_from_service_handles(handle); + continue; + } + if (!group->can_be_taken_from_.load()) + { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec->service = service; + any_exec->callback_group = group; + any_exec->node = get_node_by_group(group); + remove_service_handle_from_service_handles(handle); + return; + } + // Else, the service is no longer valid, remove it and continue + remove_service_handle_from_service_handles(handle); + } + } + std::shared_ptr get_next_ready_executable() { @@ -532,6 +694,12 @@ protected: { return any_exec; } + // Check the services to see if there are any that are ready + get_next_service(any_exec); + if (any_exec->service) + { + return any_exec; + } // If there is neither a ready timer nor subscription, return a null ptr any_exec.reset(); return any_exec; @@ -580,6 +748,8 @@ private: SubscriberHandles subscriber_handles_; typedef std::list GuardConditionHandles; GuardConditionHandles guard_condition_handles_; + typedef std::list ServiceHandles; + ServiceHandles service_handles_; }; diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7b69631..dcc91bd 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -21,9 +21,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -89,6 +91,20 @@ public: typedef std::weak_ptr CallbackGroupWeakPtr; typedef std::list CallbackGroupWeakPtrList; + /* Create and return a Client. */ + template + typename rclcpp::client::Client::SharedPtr + create_client(std::string service_name); + + /* Create and return a Service. */ + template + typename rclcpp::service::Service::SharedPtr + create_service( + std::string service_name, + std::function &, + std::shared_ptr&)> callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + private: RCLCPP_DISABLE_COPY(Node); @@ -106,6 +122,7 @@ private: size_t number_of_subscriptions_; size_t number_of_timers_; + size_t number_of_services_; }; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index 22856df..002360f 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -23,6 +23,7 @@ #include #include +#include #include @@ -41,7 +42,7 @@ Node::Node(std::string node_name) Node::Node(std::string node_name, context::Context::SharedPtr context) : name_(node_name), context_(context), - number_of_subscriptions_(0), number_of_timers_(0) + number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) { node_handle_ = ::ros_middleware_interface::create_node(); using rclcpp::callback_group::CallbackGroupType; @@ -162,4 +163,65 @@ Node::create_wall_timer( group); } +template +typename client::Client::SharedPtr +Node::create_client(std::string service_name) +{ + namespace rmi = ::ros_middleware_interface; + + auto &service_type_support_handle = rmi::get_service_type_support_handle(); + + auto client_handle = rmi::create_client(this->node_handle_, + service_type_support_handle, + service_name.c_str()); + + using namespace rclcpp::client; + + auto cli = Client::make_shared(client_handle, + service_name); + + return cli; +} + +template +typename service::Service::SharedPtr +Node::create_service( + std::string service_name, + std::function &, + std::shared_ptr&)> callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + namespace rmi = ::ros_middleware_interface; + + auto &service_type_support_handle = rmi::get_service_type_support_handle(); + + auto service_handle = rmi::create_service(this->node_handle_, + service_type_support_handle, + service_name.c_str()); + + using namespace rclcpp::service; + + auto serv = Service::make_shared(service_handle, + service_name, + callback); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + + if (group) + { + if (!group_in_node(group)) + { + // TODO: use custom exception + throw std::runtime_error("Cannot create timer, group not in node."); + } + group->add_service(serv_base_ptr); + } + else + { + default_callback_group_->add_service(serv_base_ptr); + } + number_of_services_++; + + return serv; +} + #endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp new file mode 100644 index 0000000..301dba4 --- /dev/null +++ b/rclcpp/include/rclcpp/service.hpp @@ -0,0 +1,113 @@ +/* Copyright 2014 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. + */ + +#ifndef RCLCPP_RCLCPP_SERVICE_HPP_ +#define RCLCPP_RCLCPP_SERVICE_HPP_ + +#include + +#include +#include + +#include + +namespace rclcpp +{ + +// Forward declaration for friend statement +namespace node {class Node;} + +namespace service +{ + +class ServiceBase +{ + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(ServiceBase); + + ServiceBase( + ros_middleware_interface::ServiceHandle service_handle, + std::string &service_name) + : service_handle_(service_handle), service_name_(service_name) + {} + + std::string get_service_name() + { + return this->service_name_; + } + + ros_middleware_interface::ServiceHandle get_service_handle() + { + return this->service_handle_; + } + + virtual std::shared_ptr create_request() = 0; + virtual void handle_request(std::shared_ptr &request) = 0; + +private: + RCLCPP_DISABLE_COPY(ServiceBase); + + ros_middleware_interface::ServiceHandle service_handle_; + std::string service_name_; + +}; + +template +class Service : public ServiceBase +{ +public: + typedef std::function< + void(const std::shared_ptr &, + std::shared_ptr&)> CallbackType; + RCLCPP_MAKE_SHARED_DEFINITIONS(Service); + + Service( + ros_middleware_interface::ServiceHandle service_handle, + std::string &service_name, + CallbackType callback) + : ServiceBase(service_handle, service_name), callback_(callback) + {} + + std::shared_ptr create_request() + { + return std::shared_ptr(new typename ServiceT::Request()); + } + + void handle_request(std::shared_ptr &request) + { + auto typed_request = std::static_pointer_cast(request); + auto response = std::shared_ptr(new typename ServiceT::Response); + callback_(typed_request, response); + send_response(typed_request, response); + } + + void send_response( + std::shared_ptr &request, + std::shared_ptr &response) + { + ::ros_middleware_interface::send_response(get_service_handle(), request.get(), response.get()); + } + +private: + RCLCPP_DISABLE_COPY(Service); + + CallbackType callback_; +}; + +} /* namespace service */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */