diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b2d1023064..087dd751bd 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -36,9 +36,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(${PROJECT_NAME}_SRCS + src/rclcpp/action_client_intra_process_base.cpp + src/rclcpp/action_server_intra_process_base.cpp src/rclcpp/any_executable.cpp src/rclcpp/callback_group.cpp src/rclcpp/client.cpp + src/rclcpp/client_intra_process_base.cpp src/rclcpp/clock.cpp src/rclcpp/context.cpp src/rclcpp/contexts/default_context.cpp @@ -108,6 +111,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp + src/rclcpp/service_intra_process_base.cpp src/rclcpp/signal_handler.cpp src/rclcpp/subscription_base.cpp src/rclcpp/subscription_intra_process_base.cpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index f3346a067c..415a175d92 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -37,9 +37,13 @@ #include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/client_intra_process.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/intra_process_setting.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" @@ -48,6 +52,8 @@ #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcutils/logging_macros.h" + #include "rmw/error_handling.h" #include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" @@ -133,7 +139,7 @@ class ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph); RCLCPP_PUBLIC - virtual ~ClientBase() = default; + virtual ~ClientBase(); /// Take the next response for this client as a type erased pointer. /** @@ -254,6 +260,15 @@ class ClientBase rclcpp::QoS get_response_subscription_actual_qos() const; + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable(); + /// Set a callback to be called when each new response is received. /** * The callback receives a size_t which is the number of responses received @@ -358,17 +373,39 @@ class ClientBase void set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data); + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implementation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_client_id, + IntraProcessManagerWeakPtr weak_ipm); + + std::shared_ptr client_intra_process_; + std::atomic_uint ipc_sequence_number_{1}; + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; std::shared_ptr context_; rclcpp::Logger node_logger_; + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_response_callback_ before + // client_handle_, so on destruction the client is + // destroyed first. Otherwise, the rmw client callback + // would point briefly to a destroyed function. + std::function on_new_response_callback_{nullptr}; + // Declare client_handle_ after callback std::shared_ptr client_handle_; std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex callback_mutex_; - std::function on_new_response_callback_{nullptr}; + std::recursive_mutex ipc_mutex_; + bool use_intra_process_{false}; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_client_id_; }; template @@ -464,12 +501,14 @@ class Client : public ClientBase * \param[in] node_graph The node graph interface of the corresponding node. * \param[in] service_name Name of the topic to publish to. * \param[in] client_options options for the subscription. + * \param[in] ipc_setting Intra-process communication setting for the client. */ Client( rclcpp::node_interfaces::NodeBaseInterface * node_base, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, const std::string & service_name, - rcl_client_options_t & client_options) + rcl_client_options_t & client_options, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) : ClientBase(node_base, node_graph), srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { @@ -492,10 +531,27 @@ class Client : public ClientBase } rclcpp::exceptions::throw_from_rcl_error(ret, "could not create client"); } + + // Setup intra process if requested. + if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) { + create_intra_process_client(); + } } virtual ~Client() { + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a client."); + return; + } + ipm->remove_client(intra_process_client_id_); } /// Take the next response for this client. @@ -612,7 +668,7 @@ class Client : public ClientBase Promise promise; auto future = promise.get_future(); auto req_id = async_send_request_impl( - *request, + std::move(request), std::move(promise)); return FutureAndRequestId(std::move(future), req_id); } @@ -647,7 +703,7 @@ class Client : public ClientBase Promise promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( - *request, + std::move(request), std::make_tuple( CallbackType{std::forward(cb)}, shared_future, @@ -678,7 +734,7 @@ class Client : public ClientBase PromiseWithRequest promise; auto shared_future = promise.get_future().share(); auto req_id = async_send_request_impl( - *request, + request, std::make_tuple( CallbackWithRequestType{std::forward(cb)}, request, @@ -820,11 +876,33 @@ class Client : public ClientBase CallbackWithRequestTypeValueVariant>; int64_t - async_send_request_impl(const Request & request, CallbackInfoVariant value) + async_send_request_impl(SharedRequest request, CallbackInfoVariant value) { + std::lock_guard ipc_lock(ipc_mutex_); + if (use_intra_process_) { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process send called after destruction of intra process manager"); + } + bool intra_process_server_available = ipm->service_is_available(intra_process_client_id_); + + // Check if there's an intra-process server available matching this client. + // If there's not, we fall back into inter-process communication, since + // the server might be available in another process or was configured to not use IPC. + if (intra_process_server_available) { + // Send intra-process request + ipm->send_intra_process_client_request( + intra_process_client_id_, + std::make_pair(std::move(request), std::move(value))); + return ipc_sequence_number_++; + } + } + + // Send inter-process request int64_t sequence_number; std::lock_guard lock(pending_requests_mutex_); - rcl_ret_t ret = rcl_send_request(get_client_handle().get(), &request, &sequence_number); + rcl_ret_t ret = rcl_send_request(get_client_handle().get(), request.get(), &sequence_number); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send request"); } @@ -850,6 +928,40 @@ class Client : public ClientBase return value; } + void + create_intra_process_client() + { + // Check if the QoS is compatible with intra-process. + auto qos_profile = get_response_subscription_actual_qos(); + + if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + if (qos_profile.depth() == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + // Create a ClientIntraProcess which will be given to the intra-process manager. + using ClientIntraProcessT = rclcpp::experimental::ClientIntraProcess; + + client_intra_process_ = std::make_shared( + context_, + this->get_service_name(), + qos_profile); + + // Add it to the intra process manager. + using rclcpp::experimental::IntraProcessManager; + auto ipm = context_->get_sub_context(); + uint64_t intra_process_client_id = ipm->add_intra_process_client(client_intra_process_); + this->setup_intra_process(intra_process_client_id, ipm); + } + RCLCPP_DISABLE_COPY(Client) std::unordered_map< diff --git a/rclcpp/include/rclcpp/create_client.hpp b/rclcpp/include/rclcpp/create_client.hpp index 00e6ff3c0e..78dc681877 100644 --- a/rclcpp/include/rclcpp/create_client.hpp +++ b/rclcpp/include/rclcpp/create_client.hpp @@ -65,7 +65,8 @@ create_client( std::shared_ptr node_services, const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) { rcl_client_options_t options = rcl_client_get_default_options(); options.qos = qos_profile; @@ -74,7 +75,8 @@ create_client( node_base.get(), node_graph, service_name, - options); + options, + ipc_setting); auto cli_base_ptr = std::dynamic_pointer_cast(cli); node_services->add_client(cli_base_ptr, group); diff --git a/rclcpp/include/rclcpp/create_service.hpp b/rclcpp/include/rclcpp/create_service.hpp index 42c253a526..031daf42a0 100644 --- a/rclcpp/include/rclcpp/create_service.hpp +++ b/rclcpp/include/rclcpp/create_service.hpp @@ -63,7 +63,8 @@ create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) { rclcpp::AnyServiceCallback any_service_callback; any_service_callback.set(std::forward(callback)); @@ -72,8 +73,8 @@ create_service( service_options.qos = qos_profile; auto serv = Service::make_shared( - node_base->get_shared_rcl_node_handle(), - service_name, any_service_callback, service_options); + node_base, + service_name, any_service_callback, service_options, ipc_setting); auto serv_base_ptr = std::dynamic_pointer_cast(serv); node_services->add_service(serv_base_ptr, group); return serv; diff --git a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp index 9098bfe695..765b0132b1 100644 --- a/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp +++ b/rclcpp/include/rclcpp/detail/resolve_use_intra_process.hpp @@ -26,12 +26,12 @@ namespace detail { /// Return whether or not intra process is enabled, resolving "NodeDefault" if needed. -template +template bool -resolve_use_intra_process(const OptionsT & options, const NodeBaseT & node_base) +resolve_use_intra_process(IntraProcessSetting ipc_setting, const NodeBaseT & node_base) { bool use_intra_process; - switch (options.use_intra_process_comm) { + switch (ipc_setting) { case IntraProcessSetting::Enable: use_intra_process = true; break; diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index 3f41de469c..f9b75eb7cf 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -260,6 +260,16 @@ class EventHandler : public EventHandlerBase } } + ~EventHandler() + { + // Since the rmw event listener holds a reference to the + // "on ready" callback, we need to clear it on destruction of this class. + // This clearing is not needed for other rclcpp entities like pub/subs, since + // they do own the underlying rmw entities, which are destroyed + // on their rclcpp destructors, thus no risk of dangling pointers. + clear_on_ready_callback(); + } + /// Take data so that the callback cannot be scheduled again std::shared_ptr take_data() override diff --git a/rclcpp/include/rclcpp/experimental/action_client_intra_process.hpp b/rclcpp/include/rclcpp/experimental/action_client_intra_process.hpp new file mode 100644 index 0000000000..17a5694002 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/action_client_intra_process.hpp @@ -0,0 +1,282 @@ +// Copyright 2022 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__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_HPP_ + +#include +#include +#include +#include +#include +#include +#include // NOLINT, cpplint doesn't think this is a cpp std header + +#include "rcutils/logging_macros.h" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/action_client_intra_process_base.hpp" + +typedef struct rcl_action_client_depth_s +{ + size_t goal_service_depth; + size_t result_service_depth; + size_t cancel_service_depth; + size_t feedback_topic_depth; + size_t status_topic_depth; +} rcl_action_client_depth_t; + +namespace rclcpp +{ +namespace experimental +{ + +template +class ActionClientIntraProcess : public ActionClientIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ActionClientIntraProcess) + + // Useful aliases for the action client data types + using ResponseCallback = std::function)>; + using GoalResponse = typename ActionT::Impl::SendGoalService::Response; + using GoalResponseSharedPtr = typename std::shared_ptr; + using ResultResponse = typename ActionT::Impl::GetResultService::Response; + using ResultResponseSharedPtr = typename std::shared_ptr; + using FeedbackMessage = typename ActionT::Impl::FeedbackMessage; + using FeedbackSharedPtr = typename std::shared_ptr; + using CancelGoalSharedPtr = typename std::shared_ptr; + using GoalStatusSharedPtr = typename std::shared_ptr; + + ActionClientIntraProcess( + rclcpp::Context::SharedPtr context, + const std::string & action_name, + const rcl_action_client_depth_t & qos_history, + ResponseCallback goal_status_callback, + ResponseCallback feedback_callback) + : goal_status_callback_(goal_status_callback), + feedback_callback_(feedback_callback), + ActionClientIntraProcessBase( + context, + action_name, + QoS(qos_history.goal_service_depth)) + { + // Create the intra-process buffers + goal_response_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.goal_service_depth)); + + result_response_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.result_service_depth)); + + status_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.status_topic_depth)); + + feedback_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.feedback_topic_depth)); + + cancel_response_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.cancel_service_depth)); + } + + virtual ~ActionClientIntraProcess() = default; + + bool is_ready(rcl_wait_set_t * wait_set) + { + (void) wait_set; + + return is_feedback_ready_ || + is_status_ready_ || + is_goal_response_ready_ || + is_cancel_response_ready_ || + is_result_response_ready_; + } + + // Store responses callbacks. + // We don't use mutex to protect these callbacks since they + // are called always after they are set. + void store_goal_response_callback(ResponseCallback callback) + { + goal_response_callback_ = callback; + } + + void store_cancel_goal_callback(ResponseCallback callback) + { + cancel_goal_callback_ = callback; + } + + void store_result_response_callback(ResponseCallback callback) + { + result_response_callback_ = callback; + } + + // Store responses from server + void store_ipc_action_goal_response(GoalResponseSharedPtr goal_response) + { + goal_response_buffer_->add(std::move(goal_response)); + gc_.trigger(); + is_goal_response_ready_ = true; + invoke_on_ready_callback(EventType::GoalResponse); + } + + void store_ipc_action_result_response(ResultResponseSharedPtr result_response) + { + result_response_buffer_->add(std::move(result_response)); + gc_.trigger(); + is_result_response_ready_ = true; + invoke_on_ready_callback(EventType::ResultResponse); + } + + void store_ipc_action_cancel_response(CancelGoalSharedPtr cancel_response) + { + cancel_response_buffer_->add(std::move(cancel_response)); + gc_.trigger(); + is_cancel_response_ready_ = true; + invoke_on_ready_callback(EventType::CancelResponse); + } + + void store_ipc_action_feedback(FeedbackSharedPtr feedback) + { + feedback_buffer_->add(std::move(feedback)); + gc_.trigger(); + is_feedback_ready_ = true; + invoke_on_ready_callback(EventType::FeedbackReady); + } + + void store_ipc_action_goal_status(GoalStatusSharedPtr status) + { + status_buffer_->add(std::move(status)); + gc_.trigger(); + is_status_ready_ = true; + invoke_on_ready_callback(EventType::StatusReady); + } + + std::shared_ptr + take_data() override + { + if (is_goal_response_ready_) { + auto data = std::move(goal_response_buffer_->consume()); + return std::static_pointer_cast(data); + } else if (is_result_response_ready_) { + auto data = std::move(result_response_buffer_->consume()); + return std::static_pointer_cast(data); + } else if (is_cancel_response_ready_) { + auto data = std::move(cancel_response_buffer_->consume()); + return std::static_pointer_cast(data); + } else if (is_feedback_ready_) { + auto data = std::move(feedback_buffer_->consume()); + return std::static_pointer_cast(data); + } else if (is_status_ready_) { + auto data = std::move(status_buffer_->consume()); + return std::static_pointer_cast(data); + } else { + throw std::runtime_error("Taking data from intra-process action client but nothing is ready"); + } + } + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + // Mark as ready the event type from which we want to take data + switch (static_cast(id)) { + case EventType::ResultResponse: + is_result_response_ready_ = true; + break; + case EventType::CancelResponse: + is_cancel_response_ready_ = true; + break; + case EventType::GoalResponse: + is_goal_response_ready_ = true; + break; + case EventType::FeedbackReady: + is_feedback_ready_ = true; + break; + case EventType::StatusReady: + is_status_ready_ = true; + break; + } + + return take_data(); + } + + + void execute(std::shared_ptr & data) + { + // How to handle case when more than one flag is ready? + // For example, feedback and status are both ready, guard condition triggered + // twice, but we process a single entity here. + // On the default executor using a waitset, waitables are checked twice if ready, + // so that fixes the issue. Check if this is a problem with EventsExecutor. + if (!data) { + throw std::runtime_error("'data' is empty"); + } + + if (is_goal_response_ready_) { + is_goal_response_ready_ = false; + goal_response_callback_(std::move(data)); + } else if (is_result_response_ready_) { + is_result_response_ready_ = false; + result_response_callback_(std::move(data)); + } else if (is_cancel_response_ready_) { + is_cancel_response_ready_ = false; + cancel_goal_callback_(std::move(data)); + } else if (is_feedback_ready_) { + is_feedback_ready_ = false; + feedback_callback_(std::move(data)); + } else if (is_status_ready_) { + is_status_ready_ = false; + goal_status_callback_(std::move(data)); + } else { + throw std::runtime_error("Executing intra-process action client but nothing is ready"); + } + } + +protected: + ResponseCallback goal_response_callback_; + ResponseCallback result_response_callback_; + ResponseCallback cancel_goal_callback_; + ResponseCallback goal_status_callback_; + ResponseCallback feedback_callback_; + + // Create buffers to store data coming from server + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + GoalResponseSharedPtr>::UniquePtr goal_response_buffer_; + + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + ResultResponseSharedPtr>::UniquePtr result_response_buffer_; + + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + FeedbackSharedPtr>::UniquePtr feedback_buffer_; + + rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + GoalStatusSharedPtr>::UniquePtr status_buffer_; + + rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + CancelGoalSharedPtr>::UniquePtr cancel_response_buffer_; + + std::atomic is_feedback_ready_{false}; + std::atomic is_status_ready_{false}; + std::atomic is_goal_response_ready_{false}; + std::atomic is_cancel_response_ready_{false}; + std::atomic is_result_response_ready_{false}; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp new file mode 100644 index 0000000000..79cfa806a5 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/action_client_intra_process_base.hpp @@ -0,0 +1,255 @@ +// Copyright 2022 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__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_BASE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class ActionClientIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ActionClientIntraProcessBase) + + // The action client event types + enum class EventType : std::size_t + { + ResultResponse, + CancelResponse, + GoalResponse, + FeedbackReady, + StatusReady, + }; + + RCLCPP_PUBLIC + ActionClientIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & action_name, + const rclcpp::QoS & qos_profile) + : gc_(context), + action_name_(action_name), + qos_profile_(qos_profile) + {} + + virtual ~ActionClientIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual + std::shared_ptr + take_data() = 0; + + virtual + std::shared_ptr + take_data_by_entity_id(size_t id) = 0; + + virtual void + execute(std::shared_ptr & data) = 0; + + RCLCPP_PUBLIC + const char * + get_action_name() const; + + RCLCPP_PUBLIC + QoS + get_actual_qos() const; + + /// Set a callback to be called when each new response arrives. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. In this case they identify event types. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bound to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new request is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_event_type(EventType::ResultResponse, callback); + set_callback_to_event_type(EventType::CancelResponse, callback); + set_callback_to_event_type(EventType::GoalResponse, callback); + set_callback_to_event_type(EventType::FeedbackReady, callback); + set_callback_to_event_type(EventType::StatusReady, callback); + } + + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + event_type_to_on_ready_callback_.clear(); + } + +protected: + std::recursive_mutex reentrant_mutex_; + rclcpp::GuardCondition gc_; + + // Action client on ready callbacks and unread count. + // These callbacks can be set by the user to be notified about new events + // on the action client like a new goal response, result response, etc. + // These events have a counter associated with them, counting the amount of events + // that happened before having assigned a callback for them. + using EventTypeOnReadyCallback = std::function; + using CallbackUnreadCountPair = std::pair; + + // Map the different action client event types to their callbacks and unread count. + std::unordered_map event_type_to_on_ready_callback_; + + // Invoke the callback to be called when the action client has a new event + void + invoke_on_ready_callback(EventType event_type) + { + std::lock_guard lock(reentrant_mutex_); + + // Search for a callback for this event type + auto it = event_type_to_on_ready_callback_.find(event_type); + + if (it != event_type_to_on_ready_callback_.end()) { + auto & on_ready_callback = it->second.first; + // If there's a callback associated with this event type, call it + if (on_ready_callback) { + on_ready_callback(1); + } else { + // We don't have a callback for this event type yet, + // increase its event counter. + auto & event_type_unread_count = it->second.second; + event_type_unread_count++; + } + } else { + // No entries found for this event type, create one + // with an emtpy callback and one unread event. + event_type_to_on_ready_callback_.emplace(event_type, std::make_pair(nullptr, 1)); + } + } + +private: + std::string action_name_; + QoS qos_profile_; + + void set_callback_to_event_type( + EventType event_type, + std::function callback) + { + auto new_callback = create_event_type_callback(callback, event_type); + + std::lock_guard lock(reentrant_mutex_); + + // Check if we have already an entry for this event type + auto it = event_type_to_on_ready_callback_.find(event_type); + + if (it != event_type_to_on_ready_callback_.end()) { + // We have an entry for this event type, check how many + // events of this event type happened so far. + auto & event_type_unread_count = it->second.second; + if (event_type_unread_count) { + new_callback(event_type_unread_count); + } + event_type_unread_count = 0; + // Set the new callback for this event type + auto & event_type_on_ready_callback = it->second.first; + event_type_on_ready_callback = new_callback; + } else { + // We had no entries for this event type, create one + // with the new callback and zero as unread count. + event_type_to_on_ready_callback_.emplace(event_type, std::make_pair(new_callback, 0)); + } + } + + std::function + create_event_type_callback( + std::function callback, + EventType event_type) + { + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, event_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(event_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp_action"), + "rclcpp::experimental::ActionClientIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp_action"), + "rclcpp::experimental::ActionClientIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + return new_callback; + } +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/action_server_intra_process.hpp b/rclcpp/include/rclcpp/experimental/action_server_intra_process.hpp new file mode 100644 index 0000000000..156b8db563 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/action_server_intra_process.hpp @@ -0,0 +1,236 @@ +// Copyright 2022 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__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_HPP_ + +#include +#include +#include +#include +#include +#include +#include // NOLINT, cpplint doesn't think this is a cpp std header + +#include "rcutils/logging_macros.h" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/action_server_intra_process_base.hpp" + +typedef struct rcl_action_server_depth_s +{ + size_t goal_service_depth; + size_t cancel_service_depth; + size_t result_service_depth; +} rcl_action_server_depth_t; + +namespace rclcpp +{ +namespace experimental +{ + +template +class ActionServerIntraProcess : public ActionServerIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ActionServerIntraProcess) + + // Useful aliases for the action server data types + using ResponseCallback = std::function response)>; + + using GoalRequest = typename ActionT::Impl::SendGoalService::Request; + using GoalRequestSharedPtr = typename std::shared_ptr; + using GoalRequestDataPair = typename std::pair; + using GoalRequestDataPairSharedPtr = typename std::shared_ptr; + using GoalRequestCallback = std::function; + + using ResultRequest = typename ActionT::Impl::GetResultService::Request; + using ResultRequestSharedPtr = typename std::shared_ptr; + using ResultRequestDataPair = typename std::pair; + using ResultRequestDataPairSharedPtr = typename std::shared_ptr; + using ResultRequestCallback = std::function; + + using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; + using CancelRequestSharedPtr = typename std::shared_ptr; + using CancelRequestDataPair = typename std::pair; + using CancelRequestDataPairSharedPtr = typename std::shared_ptr; + using CancelGoalCallback = std::function; + + ActionServerIntraProcess( + rclcpp::Context::SharedPtr context, + const std::string & action_name, + const rcl_action_server_depth_t & qos_history, + GoalRequestCallback goal_request_received_callback, + CancelGoalCallback cancel_request_received_callback, + ResultRequestCallback result_request_received_callback) + : execute_goal_request_received_(goal_request_received_callback), + execute_cancel_request_received_(cancel_request_received_callback), + execute_result_request_received_(result_request_received_callback), + ActionServerIntraProcessBase( + context, + action_name, + QoS(qos_history.goal_service_depth)) + { + // Create the intra-process buffers + goal_request_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.goal_service_depth)); + + result_request_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.result_service_depth)); + + cancel_request_buffer_ = + rclcpp::experimental::create_service_intra_process_buffer( + QoS(qos_history.cancel_service_depth)); + } + + virtual ~ActionServerIntraProcess() = default; + + bool is_ready(rcl_wait_set_t * wait_set) + { + (void)wait_set; + + return goal_request_ready_ || + cancel_request_ready_ || + result_request_ready_ || + goal_expired_; + } + + void store_ipc_action_goal_request( + uint64_t ipc_action_client_id, + GoalRequestSharedPtr goal_request) + { + goal_request_buffer_->add( + std::make_pair(ipc_action_client_id, std::move(goal_request))); + gc_.trigger(); + goal_request_ready_ = true; + invoke_on_ready_callback(EventType::GoalRequest); + } + + void store_ipc_action_result_request( + uint64_t ipc_action_client_id, + ResultRequestSharedPtr result_request) + { + result_request_buffer_->add( + std::make_pair(ipc_action_client_id, std::move(result_request))); + gc_.trigger(); + result_request_ready_ = true; + invoke_on_ready_callback(EventType::ResultRequest); + } + + void store_ipc_action_cancel_request( + uint64_t ipc_action_client_id, + CancelRequestSharedPtr cancel_request) + { + cancel_request_buffer_->add( + std::make_pair(ipc_action_client_id, std::move(cancel_request))); + gc_.trigger(); + cancel_request_ready_ = true; + invoke_on_ready_callback(EventType::CancelGoal); + } + + std::shared_ptr + take_data() override + { + if (goal_request_ready_) { + auto data = std::make_shared( + std::move(goal_request_buffer_->consume())); + return std::static_pointer_cast(data); + } else if (cancel_request_ready_) { + auto data = std::make_shared( + std::move(cancel_request_buffer_->consume())); + return std::static_pointer_cast(data); + } else if (result_request_ready_) { + auto data = std::make_shared( + std::move(result_request_buffer_->consume())); + return std::static_pointer_cast(data); + } else if (goal_expired_) { + return nullptr; + } else { + throw std::runtime_error("Taking data from action server but nothing is ready"); + } + return nullptr; + } + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + // Mark as ready the event type from which we want to take data + switch (static_cast(id)) { + case EventType::GoalRequest: + goal_request_ready_ = true; + break; + case EventType::CancelGoal: + cancel_request_ready_ = true; + break; + case EventType::ResultRequest: + result_request_ready_ = true; + break; + } + + return take_data(); + } + + void execute(std::shared_ptr & data) + { + if (!data && !goal_expired_) { + throw std::runtime_error("'data' is empty"); + } + + if (goal_request_ready_) { + goal_request_ready_ = false; + auto goal_request_data = std::static_pointer_cast(data); + execute_goal_request_received_(std::move(goal_request_data)); + } else if (cancel_request_ready_) { + cancel_request_ready_ = false; + auto cancel_goal_data = std::static_pointer_cast(data); + execute_cancel_request_received_(std::move(cancel_goal_data)); + } else if (result_request_ready_) { + result_request_ready_ = false; + auto result_request_data = std::static_pointer_cast(data); + execute_result_request_received_(std::move(result_request_data)); + } else if (goal_expired_) { + // TODO(mauropasse): Handle goal expired case + // execute_check_expired_goals(); + } else { + throw std::runtime_error("Executing action server but nothing is ready"); + } + } + +protected: + // Create one buffer per type of client request + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + GoalRequestDataPair>::UniquePtr goal_request_buffer_; + + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + ResultRequestDataPair>::UniquePtr result_request_buffer_; + + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + CancelRequestDataPair>::UniquePtr cancel_request_buffer_; + + std::atomic goal_request_ready_{false}; + std::atomic cancel_request_ready_{false}; + std::atomic result_request_ready_{false}; + std::atomic goal_expired_{false}; + + GoalRequestCallback execute_goal_request_received_; + CancelGoalCallback execute_cancel_request_received_; + ResultRequestCallback execute_result_request_received_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/action_server_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/action_server_intra_process_base.hpp new file mode 100644 index 0000000000..0d3ce32ac4 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/action_server_intra_process_base.hpp @@ -0,0 +1,252 @@ +// Copyright 2022 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__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_BASE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/action_client_intra_process_base.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class ActionServerIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ActionServerIntraProcessBase) + + // The action server event types + enum class EventType : std::size_t + { + GoalRequest, + CancelGoal, + ResultRequest, + }; + + RCLCPP_PUBLIC + ActionServerIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & action_name, + const rclcpp::QoS & qos_profile) + : gc_(context), + action_name_(action_name), + qos_profile_(qos_profile) + {} + + virtual ~ActionServerIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual + std::shared_ptr + take_data() = 0; + + virtual + std::shared_ptr + take_data_by_entity_id(size_t id) = 0; + + virtual void + execute(std::shared_ptr & data) = 0; + + RCLCPP_PUBLIC + const char * + get_action_name() const; + + RCLCPP_PUBLIC + QoS + get_actual_qos() const; + + /// Set a callback to be called when each new request arrives. + /** + * The callback receives a size_t which is the number of requests received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if requests were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. In this case they identify event types. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bound to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new request is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_event_type(EventType::GoalRequest, callback); + set_callback_to_event_type(EventType::CancelGoal, callback); + set_callback_to_event_type(EventType::ResultRequest, callback); + } + + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + event_type_to_on_ready_callback_.clear(); + } + +protected: + std::recursive_mutex reentrant_mutex_; + rclcpp::GuardCondition gc_; + + // Action server on ready callbacks and unread count. + // These callbacks can be set by the user to be notified about new events + // on the action server like a new goal request, result request or a cancel goal request. + // These events have a counter associated with them, counting the amount of events + // that happened before having assigned a callback for them. + using EventTypeOnReadyCallback = std::function; + using CallbackUnreadCountPair = std::pair; + + // Map the different action server event types to their callbacks and unread count. + std::unordered_map event_type_to_on_ready_callback_; + + // Invoke the callback to be called when the action server has a new event + void + invoke_on_ready_callback(EventType event_type) + { + std::lock_guard lock(reentrant_mutex_); + + // Search for a callback for this event type + auto it = event_type_to_on_ready_callback_.find(event_type); + + if (it != event_type_to_on_ready_callback_.end()) { + auto & on_ready_callback = it->second.first; + // If there's a callback associated with this event type, call it + if (on_ready_callback) { + on_ready_callback(1); + } else { + // We don't have a callback for this event type yet, + // increase its event counter. + auto & event_type_unread_count = it->second.second; + event_type_unread_count++; + } + } else { + // No entries found for this event type, create one + // with an emtpy callback and one unread event. + event_type_to_on_ready_callback_.emplace(event_type, std::make_pair(nullptr, 1)); + } + } + +private: + std::string action_name_; + QoS qos_profile_; + + void set_callback_to_event_type( + EventType event_type, + std::function callback) + { + auto new_callback = create_event_type_callback(callback, event_type); + + std::lock_guard lock(reentrant_mutex_); + + // Check if we have already an entry for this event type + auto it = event_type_to_on_ready_callback_.find(event_type); + + if (it != event_type_to_on_ready_callback_.end()) { + // We have an entry for this event type, check how many + // events of this event type happened so far. + auto & event_type_unread_count = it->second.second; + if (event_type_unread_count) { + new_callback(event_type_unread_count); + } + event_type_unread_count = 0; + // Set the new callback for this event type + auto & event_type_on_ready_callback = it->second.first; + event_type_on_ready_callback = new_callback; + } else { + // We had no entries for this event type, create one + // with the new callback and zero as unread count. + event_type_to_on_ready_callback_.emplace(event_type, std::make_pair(new_callback, 0)); + } + } + + std::function + create_event_type_callback( + std::function callback, + EventType event_type) + { + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, event_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(event_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp_action"), + "rclcpp::experimental::ActionServerIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp_action"), + "rclcpp::experimental::ActionServerIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + return new_callback; + } +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp index 05092bb23b..b6205743c6 100644 --- a/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/buffers/intra_process_buffer.hpp @@ -239,6 +239,51 @@ class TypedIntraProcessBuffer : public IntraProcessBuffer +class ServiceIntraProcessBuffer : public IntraProcessBufferBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ServiceIntraProcessBuffer) + + virtual ~ServiceIntraProcessBuffer() {} + + explicit + ServiceIntraProcessBuffer( + std::unique_ptr> buffer_impl) + { + buffer_ = std::move(buffer_impl); + } + + bool use_take_shared_method() const override + { + return false; + } + + bool has_data() const override + { + return buffer_->has_data(); + } + + void clear() override + { + buffer_->clear(); + } + + void add(BufferT && msg) + { + buffer_->enqueue(std::move(msg)); + } + + BufferT + consume() + { + return buffer_->dequeue(); + } + +private: + std::unique_ptr> buffer_; +}; + } // namespace buffers } // namespace experimental } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/client_intra_process.hpp b/rclcpp/include/rclcpp/experimental/client_intra_process.hpp new file mode 100644 index 0000000000..fb30e6ee81 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/client_intra_process.hpp @@ -0,0 +1,142 @@ +// Copyright 2021 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__EXPERIMENTAL__CLIENT_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_HPP_ + +#include +#include +#include +#include +#include +#include +#include // NOLINT, cpplint doesn't think this is a cpp std header + +#include "rcutils/logging_macros.h" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/client_intra_process_base.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +template +class ClientIntraProcess : public ClientIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ClientIntraProcess) + + using SharedRequest = typename ServiceT::Request::SharedPtr; + using SharedResponse = typename ServiceT::Response::SharedPtr; + + using Promise = std::promise; + using PromiseWithRequest = std::promise>; + + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; + + using CallbackType = std::function; + using CallbackWithRequestType = std::function; + + using CallbackTypeValueVariant = std::tuple; + using CallbackWithRequestTypeValueVariant = std::tuple< + CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + + using ServiceResponse = std::pair; + + ClientIntraProcess( + rclcpp::Context::SharedPtr context, + const std::string & service_name, + const rclcpp::QoS & qos_profile) + : ClientIntraProcessBase(context, service_name, qos_profile) + { + // Create the intra-process buffer. + buffer_ = rclcpp::experimental::create_service_intra_process_buffer< + ServiceResponse>(qos_profile); + } + + virtual ~ClientIntraProcess() = default; + + bool + is_ready(rcl_wait_set_t * wait_set) + { + (void) wait_set; + return buffer_->has_data(); + } + + void + store_intra_process_response(ServiceResponse && response) + { + buffer_->add(std::move(response)); + gc_.trigger(); + invoke_on_new_response(); + } + + std::shared_ptr + take_data() override + { + auto data = std::make_shared(std::move(buffer_->consume())); + return std::static_pointer_cast(data); + } + + void execute(std::shared_ptr & data) + { + if (!data) { + throw std::runtime_error("'data' is empty"); + } + + auto data_ptr = std::static_pointer_cast(data); + auto & typed_response = data_ptr->first; + auto & value = data_ptr->second; + + if (std::holds_alternative(value)) { + auto & promise = std::get(value); + promise.set_value(std::move(typed_response)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + promise.set_value(std::move(typed_response)); + callback(std::move(future)); + } else if (std::holds_alternative(value)) { + auto & inner = std::get(value); + const auto & callback = std::get(inner); + auto & promise = std::get(inner); + auto & future = std::get(inner); + auto & request = std::get(inner); + promise.set_value(std::make_pair(std::move(request), std::move(typed_response))); + callback(std::move(future)); + } + } + +protected: + using BufferUniquePtr = + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + ServiceResponse>::UniquePtr; + + BufferUniquePtr buffer_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/client_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/client_intra_process_base.hpp new file mode 100644 index 0000000000..5c59d8e421 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/client_intra_process_base.hpp @@ -0,0 +1,196 @@ +// Copyright 2021 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__EXPERIMENTAL__CLIENT_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_BASE_HPP_ + +#include +#include +#include +#include + +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/context.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class ClientIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ClientIntraProcessBase) + + enum class EntityType : std::size_t + { + Client, + }; + + RCLCPP_PUBLIC + ClientIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & service_name, + const rclcpp::QoS & qos_profile) + : gc_(context), service_name_(service_name), qos_profile_(qos_profile) + {} + + virtual ~ClientIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual + std::shared_ptr + take_data() = 0; + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + + virtual void + execute(std::shared_ptr & data) = 0; + + RCLCPP_PUBLIC + const char * + get_service_name() const; + + RCLCPP_PUBLIC + QoS + get_actual_qos() const; + + /// Set a callback to be called when each new response arrives. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bound to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new response is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Client)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::ClientIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::ClientIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + on_new_response_callback_ = new_callback; + + if (unread_count_ > 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { + on_new_response_callback_(unread_count_); + } else { + // Use qos profile depth as upper bound for unread_count_ + on_new_response_callback_(std::min(unread_count_, qos_profile_.depth())); + } + unread_count_ = 0; + } + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + on_new_response_callback_ = nullptr; + } + +protected: + std::recursive_mutex reentrant_mutex_; + rclcpp::GuardCondition gc_; + std::function on_new_response_callback_ {nullptr}; + size_t unread_count_{0}; + + void + invoke_on_new_response() + { + std::lock_guard lock(reentrant_mutex_); + if (on_new_response_callback_) { + on_new_response_callback_(1); + } else { + unread_count_++; + } + } + +private: + std::string service_name_; + QoS qos_profile_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp index 4d7668b964..262aec96a3 100644 --- a/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/create_intra_process_buffer.hpp @@ -92,6 +92,26 @@ create_intra_process_buffer( return buffer; } +template +typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer::UniquePtr +create_service_intra_process_buffer(const rclcpp::QoS & qos) +{ + using rclcpp::experimental::buffers::RingBufferImplementation; + + size_t buffer_size = qos.depth(); + auto buffer_impl = std::make_unique>(buffer_size); + + using rclcpp::experimental::buffers::ServiceIntraProcessBuffer; + typename ServiceIntraProcessBuffer::UniquePtr buffer; + + // Construct the intra_process_buffer + buffer = + std::make_unique>( + std::move(buffer_impl)); + + return buffer; +} + } // namespace experimental } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp index 148ede66ea..dd5b1ebe63 100644 --- a/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp +++ b/rclcpp/include/rclcpp/experimental/executors/events_executor/events_executor.hpp @@ -243,6 +243,11 @@ class EventsExecutor : public rclcpp::Executor std::function create_waitable_callback(const rclcpp::Waitable * waitable_id); + /// Utility to add the notify waitable to an entities collection + void + add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection); + /// Searches for the provided entity_id in the collection and returns the entity if valid template typename CollectionType::EntitySharedPtr @@ -269,9 +274,12 @@ class EventsExecutor : public rclcpp::Executor rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_; std::shared_ptr entities_collector_; - std::shared_ptr current_entities_collection_; std::shared_ptr notify_waitable_; + /// Mutex to protect the current_entities_collection_ + std::recursive_mutex collection_mutex_; + std::shared_ptr current_entities_collection_; + /// Flag used to reduce the number of unnecessary waitable events std::atomic notify_waitable_event_pushed_ {false}; diff --git a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp index 11f2dda6a4..7cd91a809c 100644 --- a/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/experimental/intra_process_manager.hpp @@ -28,7 +28,15 @@ #include #include "rclcpp/allocator/allocator_deleter.hpp" +#include "rclcpp/experimental/action_client_intra_process.hpp" +#include "rclcpp/experimental/action_client_intra_process_base.hpp" +#include "rclcpp/experimental/action_server_intra_process.hpp" +#include "rclcpp/experimental/action_server_intra_process_base.hpp" +#include "rclcpp/experimental/client_intra_process.hpp" +#include "rclcpp/experimental/client_intra_process_base.hpp" #include "rclcpp/experimental/ros_message_intra_process_buffer.hpp" +#include "rclcpp/experimental/service_intra_process.hpp" +#include "rclcpp/experimental/service_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/experimental/subscription_intra_process_buffer.hpp" @@ -97,10 +105,10 @@ class IntraProcessManager RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager) RCLCPP_PUBLIC - IntraProcessManager(); + IntraProcessManager() = default; RCLCPP_PUBLIC - virtual ~IntraProcessManager(); + virtual ~IntraProcessManager() = default; /// Register a subscription with the manager, returns subscriptions unique id. /** @@ -116,6 +124,46 @@ class IntraProcessManager uint64_t add_subscription(rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr subscription); + /// Register an intra-process client with the manager, returns the client unique id. + /** + * \param client the ClientIntraProcessBase to register. + * \return an unsigned 64-bit integer which is the client's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_intra_process_client(rclcpp::experimental::ClientIntraProcessBase::SharedPtr client); + + /// Register an intra-process service with the manager, returns the service unique id. + /** + * \param service the Service IntraProcessBase to register. + * \return an unsigned 64-bit integer which is the service's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_intra_process_service(rclcpp::experimental::ServiceIntraProcessBase::SharedPtr service); + + /// Register an intra-process action client with the manager, + /// returns the action client unique id. + /** + * \param client the ActionClientIntraProcessBase to register. + * \return an unsigned 64-bit integer which is the action client's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_intra_process_action_client( + rclcpp::experimental::ActionClientIntraProcessBase::SharedPtr client); + + /// Register an intra-process action service with the manager, + /// returns the action service unique id. + /** + * \param service the ActionServerIntraProcessBase to register. + * \return an unsigned 64-bit integer which is the action service's unique id. + */ + RCLCPP_PUBLIC + uint64_t + add_intra_process_action_server( + rclcpp::experimental::ActionServerIntraProcessBase::SharedPtr service); + /// Unregister a subscription using the subscription's unique id. /** * This method does not allocate memory. @@ -126,6 +174,38 @@ class IntraProcessManager void remove_subscription(uint64_t intra_process_subscription_id); + /// Unregister a client using the client's unique id. + /** + * \param intra_process_client_id id of the client to remove. + */ + RCLCPP_PUBLIC + void + remove_client(uint64_t intra_process_client_id); + + /// Unregister a service using the service's unique id. + /** + * \param intra_process_service_id id of the service to remove. + */ + RCLCPP_PUBLIC + void + remove_service(uint64_t intra_process_service_id); + + /// Unregister an action client using the action client's unique id. + /** + * \param ipc_action_client_id id of the client to remove. + */ + RCLCPP_PUBLIC + void + remove_action_client(uint64_t ipc_action_client_id); + + /// Unregister an action server using the action server's unique id. + /** + * \param ipc_action_server_id id of the service to remove. + */ + RCLCPP_PUBLIC + void + remove_action_server(uint64_t ipc_action_server_id); + /// Register a publisher with the manager, returns the publisher unique id. /** * This method stores the publisher intra process object, together with @@ -150,6 +230,25 @@ class IntraProcessManager void remove_publisher(uint64_t intra_process_publisher_id); + // Store an intra-process action client ID along its current + // goal UUID, since later when the server process a request + // it'll use the goal UUID to retrieve the client which asked for + // the result. + RCLCPP_PUBLIC + void + store_intra_process_action_client_goal_uuid( + uint64_t ipc_action_client_id, + size_t uuid); + + // Remove an action client goal UUID entry + RCLCPP_PUBLIC + void + remove_intra_process_action_client_goal_uuid(size_t uuid); + + RCLCPP_PUBLIC + uint64_t + get_action_client_id_from_goal_uuid(size_t uuid); + /// Publishes an intra-process message, passed as a unique pointer. /** * This is one of the two methods for publishing intra-process. @@ -237,6 +336,56 @@ class IntraProcessManager } } + /// Send an intra-process client request + /** + * Using the intra-process client id, a matching intra-process service is retrieved + * which will store the request to process it asynchronously. + * + * \param intra_process_client_id the id of the client sending the request + * \param request the client's request plus callbacks if any. + */ + template + void + send_intra_process_client_request( + uint64_t intra_process_client_id, + RequestT request) + { + std::unique_lock lock(mutex_); + + auto client_it = clients_to_services_.find(intra_process_client_id); + + if (client_it == clients_to_services_.end()) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling send_intra_process_client_request for invalid or no " + "longer existing client id"); + return; + } + uint64_t service_id = client_it->second; + + auto service_it = services_.find(service_id); + if (service_it == services_.end()) { + auto cli = get_client_intra_process(intra_process_client_id); + auto warning_msg = + "Intra-process service gone out of scope. " + "Do inter-process requests.\n" + "Client service name: " + std::string(cli->get_service_name()); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), warning_msg.c_str()); + return; + } + auto service_intra_process_base = service_it->second.lock(); + if (service_intra_process_base) { + auto service = std::dynamic_pointer_cast< + rclcpp::experimental::ServiceIntraProcess>(service_intra_process_base); + if (service) { + service->store_intra_process_request( + intra_process_client_id, std::move(request)); + } + } else { + services_.erase(service_it); + } + } + template< typename MessageT, typename ROSMessageType, @@ -292,6 +441,275 @@ class IntraProcessManager } } + /// Gets an ActionClientIntraProcess from its ID + /** + * Using the intra-process client id, gets the correspoding + * ActionClientIntraProcess if exists. + * + * \param ipc_action_client_id the id of the client + * \return and ActionClientIntraProcess or nullptr if no clients match the ID + */ + template + std::shared_ptr> + get_intra_process_action_client(uint64_t ipc_action_client_id) + { + auto action_client = get_action_client_intra_process(ipc_action_client_id); + + if (action_client) { + auto ipc_action_client = std::dynamic_pointer_cast< + rclcpp::experimental::ActionClientIntraProcess>( + action_client); + if (ipc_action_client) { + return ipc_action_client; + } + } + + throw std::runtime_error("No action clients match the specified ID."); + } + + /// Gets an ActionServerIntraProcess matching an intra-process action client ID + /** + * Using the intra-process client id, a matching intra-process action service is retrieved + * if exists. + * + * \param ipc_action_client_id the id of the client matching a server + * \return and ActionServerIntraProcess or nullptr if no servers match the client ID + */ + template + std::shared_ptr> + get_matching_intra_process_action_server(uint64_t ipc_action_client_id) + { + auto action_client_it = action_clients_to_servers_.find(ipc_action_client_id); + + if (action_client_it == action_clients_to_servers_.end()) { + throw std::runtime_error("No action clients match the specified ID."); + } + + uint64_t action_service_id = action_client_it->second; + + auto service_it = action_servers_.find(action_service_id); + if (service_it == action_servers_.end()) { + throw std::runtime_error( + "There are no servers matching the intra-process action client ID."); + } + auto action_server_intra_process_base = service_it->second.lock(); + if (action_server_intra_process_base) { + auto ipc_action_service = std::dynamic_pointer_cast< + rclcpp::experimental::ActionServerIntraProcess>( + action_server_intra_process_base); + if (ipc_action_service) { + return ipc_action_service; + } + } else { + action_servers_.erase(service_it); + } + + throw std::runtime_error("No action servers match the specified ID."); + } + + /// Send an intra-process action client goal request + /** + * Using the intra-process action client id, a matching intra-process action + * server is retrieved which will store the goal request to process it asynchronously. + * + * \param ipc_action_client_id the id of the action client sending the goal request + * \param goal_request the action client's goal request data. + * \param callback the callback to be called when the server sends the goal response + */ + template + void + intra_process_action_send_goal_request( + uint64_t ipc_action_client_id, + RequestT goal_request, + std::function)> callback) + { + // First, lets store the client callback to be called when the + // server sends the goal response + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_goal_response_callback(callback); + } + + // Now lets send the goal request + auto service = get_matching_intra_process_action_server(ipc_action_client_id); + + if (service) { + service->store_ipc_action_goal_request( + ipc_action_client_id, std::move(goal_request)); + } + } + + /// Send an intra-process action client cancel request + /** + * Using the intra-process action client id, a matching intra-process action + * server is retrieved which will store the cancel request to process it asynchronously. + * + * \param ipc_action_client_id the id of the action client sending the cancel request + * \param cancel_request the action client's cancel request data. + * \param callback the callback to be called when the server sends the cancel response + */ + template + void + intra_process_action_send_cancel_request( + uint64_t ipc_action_client_id, + CancelT cancel_request, + std::function)> callback) + { + // First, lets store the client callback to be called when the + // server sends the cancel response + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_cancel_goal_callback(callback); + } + + // Now lets send the cancel request + auto service = get_matching_intra_process_action_server(ipc_action_client_id); + + if (service) { + service->store_ipc_action_cancel_request( + ipc_action_client_id, std::move(cancel_request)); + } + } + + /// Send an intra-process action client result request + /** + * Using the intra-process action client id, a matching intra-process action + * server is retrieved which will store the result request to process it asynchronously. + * + * \param ipc_action_client_id the id of the action client sending the result request + * \param result_request the action client's result request data. + */ + template + void + intra_process_action_send_result_request( + uint64_t ipc_action_client_id, + RequestT result_request, + std::function)> callback) + { + // First, lets store the client callback to be called when the + // server sends the result response + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_result_response_callback(callback); + } + + // Now lets send the result request to the server + auto service = get_matching_intra_process_action_server(ipc_action_client_id); + + if (service) { + service->store_ipc_action_result_request( + ipc_action_client_id, std::move(result_request)); + } + } + + /// Send an intra-process action server goal response + /** + * Using the intra-process action client id, an action client is found which + * will get the response of a goal request. + * + * \param ipc_action_client_id the id of the action client receiving the response + * \param goal_response the action server's goal response data. + */ + template + void + intra_process_action_send_goal_response( + uint64_t ipc_action_client_id, + ResponseT goal_response) + { + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_ipc_action_goal_response(std::move(goal_response)); + } + } + + /// Send an intra-process action server cancel response + /** + * Using the intra-process action client id, an action client is found which + * will get the response of the cancel request. + * + * \param ipc_action_client_id the id of the action client receiving the response + * \param cancel_response the action server's cancel response data. + */ + template + void + intra_process_action_send_cancel_response( + uint64_t ipc_action_client_id, + ResponseT cancel_response) + { + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_ipc_action_cancel_response(std::move(cancel_response)); + } + } + + /// Send an intra-process action server result response + /** + * Using the intra-process action client id, an action client is found which + * will get the response of a result request. + * + * \param ipc_action_client_id the id of the action client receiving the response + * \param result_response the action server's result response data. + */ + template + void + intra_process_action_send_result_response( + uint64_t ipc_action_client_id, + ResponseT result_response) + { + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_ipc_action_result_response(std::move(result_response)); + } + } + + /// Intra-process publish an action goal status + /** + * Using the intra-process action client id, an action client is found which + * will get the goal status. + * + * \param ipc_action_client_id the id of the action client receiving the goal status + * \param status_msg the status of the goal, sent by the server + */ + template + void + intra_process_action_publish_status( + uint64_t ipc_action_client_id, + StatusT status_msg) + { + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_ipc_action_goal_status(std::move(status_msg)); + } + } + + /// Intra-process publish an action goal feedback + /** + * Using the intra-process action client id, an action client is found which + * will get the goal feedback. + * + * \param ipc_action_client_id the id of the action client receiving the goal status + * \param feedback the feedback of the goal, sent by the server + */ + template + void + intra_process_action_publish_feedback( + uint64_t ipc_action_client_id, + FeedbackT feedback) + { + auto client = get_intra_process_action_client(ipc_action_client_id); + + if (client) { + client->store_ipc_action_feedback(std::move(feedback)); + } + } + /// Return true if the given rmw_gid_t matches any stored Publishers. RCLCPP_PUBLIC bool @@ -306,6 +724,30 @@ class IntraProcessManager rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr get_subscription_intra_process(uint64_t intra_process_subscription_id); + RCLCPP_PUBLIC + rclcpp::experimental::ClientIntraProcessBase::SharedPtr + get_client_intra_process(uint64_t intra_process_client_id); + + RCLCPP_PUBLIC + rclcpp::experimental::ServiceIntraProcessBase::SharedPtr + get_service_intra_process(uint64_t intra_process_service_id); + + RCLCPP_PUBLIC + bool + service_is_available(uint64_t intra_process_client_id); + + RCLCPP_PUBLIC + rclcpp::experimental::ActionClientIntraProcessBase::SharedPtr + get_action_client_intra_process(uint64_t intra_process_action_client_id); + + RCLCPP_PUBLIC + rclcpp::experimental::ActionServerIntraProcessBase::SharedPtr + get_action_server_intra_process(uint64_t intra_process_action_server_id); + + RCLCPP_PUBLIC + bool + action_server_is_available(uint64_t ipc_action_client_id); + private: struct SplittedSubscriptions { @@ -322,6 +764,24 @@ class IntraProcessManager using PublisherToSubscriptionIdsMap = std::unordered_map; + using ClientMap = + std::unordered_map; + + using ServiceMap = + std::unordered_map; + + using ClientToServiceIdsMap = + std::unordered_map; + + using ActionClientMap = + std::unordered_map; + + using ActionServerMap = + std::unordered_map; + + using ActionClientToServerIdsMap = + std::unordered_map; + RCLCPP_PUBLIC static uint64_t @@ -337,6 +797,18 @@ class IntraProcessManager rclcpp::PublisherBase::SharedPtr pub, rclcpp::experimental::SubscriptionIntraProcessBase::SharedPtr sub) const; + RCLCPP_PUBLIC + bool + can_communicate( + rclcpp::experimental::ClientIntraProcessBase::SharedPtr client, + rclcpp::experimental::ServiceIntraProcessBase::SharedPtr service) const; + + RCLCPP_PUBLIC + bool + can_communicate( + rclcpp::experimental::ActionClientIntraProcessBase::SharedPtr client, + rclcpp::experimental::ActionServerIntraProcessBase::SharedPtr service) const; + template< typename MessageT, typename Alloc, @@ -515,6 +987,14 @@ class IntraProcessManager PublisherToSubscriptionIdsMap pub_to_subs_; SubscriptionMap subscriptions_; PublisherMap publishers_; + ClientToServiceIdsMap clients_to_services_; + ClientMap clients_; + ServiceMap services_; + ActionClientMap action_clients_; + ActionServerMap action_servers_; + ActionClientToServerIdsMap action_clients_to_servers_; + + std::unordered_map clients_uuid_to_id_; mutable std::shared_timed_mutex mutex_; }; diff --git a/rclcpp/include/rclcpp/experimental/service_intra_process.hpp b/rclcpp/include/rclcpp/experimental/service_intra_process.hpp new file mode 100644 index 0000000000..9a4852a6a7 --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/service_intra_process.hpp @@ -0,0 +1,160 @@ +// Copyright 2021 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__EXPERIMENTAL__SERVICE_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" + +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/experimental/buffers/intra_process_buffer.hpp" +#include "rclcpp/experimental/client_intra_process.hpp" +#include "rclcpp/experimental/create_intra_process_buffer.hpp" +#include "rclcpp/experimental/service_intra_process_base.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +template +class ServiceIntraProcess : public ServiceIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(ServiceIntraProcess) + + using SharedRequest = typename ServiceT::Request::SharedPtr; + using SharedResponse = typename ServiceT::Response::SharedPtr; + + using Promise = std::promise; + using PromiseWithRequest = std::promise>; + + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; + + using CallbackType = std::function; + using CallbackWithRequestType = std::function; + + using CallbackTypeValueVariant = std::tuple; + using CallbackWithRequestTypeValueVariant = std::tuple< + CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + + using RequestCallbackPair = std::pair; + using ClientIDtoRequest = std::pair; + + ServiceIntraProcess( + AnyServiceCallback callback, + rclcpp::Context::SharedPtr context, + const std::string & service_name, + const rclcpp::QoS & qos_profile) + : ServiceIntraProcessBase(context, service_name, qos_profile), any_callback_(callback) + { + // Create the intra-process buffer. + buffer_ = rclcpp::experimental::create_service_intra_process_buffer< + ClientIDtoRequest>(qos_profile); + } + + virtual ~ServiceIntraProcess() = default; + + bool + is_ready(rcl_wait_set_t * wait_set) + { + (void) wait_set; + return buffer_->has_data(); + } + + void + store_intra_process_request( + uint64_t intra_process_client_id, + RequestCallbackPair request) + { + buffer_->add(std::make_pair(intra_process_client_id, std::move(request))); + gc_.trigger(); + invoke_on_new_request(); + } + + std::shared_ptr + take_data() + { + auto data = std::make_shared(std::move(buffer_->consume())); + return std::static_pointer_cast(data); + } + + void execute(std::shared_ptr & data) + { + auto ptr = std::static_pointer_cast(data); + + uint64_t intra_process_client_id = ptr->first; + SharedRequest & typed_request = ptr->second.first; + CallbackInfoVariant & value = ptr->second.second; + + SharedResponse response = any_callback_.dispatch(nullptr, nullptr, std::move(typed_request)); + + if (response) { + std::unique_lock lock(reentrant_mutex_); + + auto client_it = clients_.find(intra_process_client_id); + + if (client_it == clients_.end()) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling intra_process_service_send_response for invalid or no " + "longer existing client id"); + return; + } + + auto client_intra_process_base = client_it->second.lock(); + + if (client_intra_process_base) { + auto client = std::dynamic_pointer_cast< + rclcpp::experimental::ClientIntraProcess>( + client_intra_process_base); + client->store_intra_process_response( + std::make_pair(std::move(response), std::move(value))); + } else { + clients_.erase(client_it); + } + } + } + +protected: + using BufferUniquePtr = + typename rclcpp::experimental::buffers::ServiceIntraProcessBuffer< + ClientIDtoRequest>::UniquePtr; + + BufferUniquePtr buffer_; + + AnyServiceCallback any_callback_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/service_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/service_intra_process_base.hpp new file mode 100644 index 0000000000..a75a83226f --- /dev/null +++ b/rclcpp/include/rclcpp/experimental/service_intra_process_base.hpp @@ -0,0 +1,211 @@ +// Copyright 2021 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__EXPERIMENTAL__SERVICE_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_BASE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" + +#include "rclcpp/context.hpp" +#include "rclcpp/experimental/client_intra_process_base.hpp" +#include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/waitable.hpp" + +namespace rclcpp +{ +namespace experimental +{ + +class ServiceIntraProcessBase : public rclcpp::Waitable +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ServiceIntraProcessBase) + + enum class EntityType : std::size_t + { + Service, + }; + + RCLCPP_PUBLIC + ServiceIntraProcessBase( + rclcpp::Context::SharedPtr context, + const std::string & service_name, + const rclcpp::QoS & qos_profile) + : gc_(context), service_name_(service_name), qos_profile_(qos_profile) + {} + + virtual ~ServiceIntraProcessBase() = default; + + RCLCPP_PUBLIC + size_t + get_number_of_ready_guard_conditions() {return 1;} + + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set); + + virtual bool + is_ready(rcl_wait_set_t * wait_set) = 0; + + virtual + std::shared_ptr + take_data() = 0; + + std::shared_ptr + take_data_by_entity_id(size_t id) override + { + (void)id; + return take_data(); + } + + virtual void + execute(std::shared_ptr & data) = 0; + + RCLCPP_PUBLIC + const char * + get_service_name() const; + + RCLCPP_PUBLIC + QoS + get_actual_qos() const; + + RCLCPP_PUBLIC + void + add_intra_process_client( + rclcpp::experimental::ClientIntraProcessBase::SharedPtr client, + uint64_t client_id); + + /// Set a callback to be called when each new request arrives. + /** + * The callback receives a size_t which is the number of requests received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if requests were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bound to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new request is received. + */ + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Service)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::ServiceIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::ServiceIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + on_new_request_callback_ = new_callback; + + if (unread_count_ > 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { + on_new_request_callback_(unread_count_); + } else { + // Use qos profile depth as upper bound for unread_count_ + on_new_request_callback_(std::min(unread_count_, qos_profile_.depth())); + } + unread_count_ = 0; + } + } + + /// Unset the callback registered for new messages, if any. + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + on_new_request_callback_ = nullptr; + } + +protected: + std::recursive_mutex reentrant_mutex_; + rclcpp::GuardCondition gc_; + std::function on_new_request_callback_ {nullptr}; + size_t unread_count_{0}; + + void + invoke_on_new_request() + { + std::lock_guard lock(reentrant_mutex_); + if (on_new_request_callback_) { + on_new_request_callback_(1); + } else { + unread_count_++; + } + } + + using ClientMap = + std::unordered_map; + + ClientMap clients_; + +private: + std::string service_name_; + QoS qos_profile_; +}; + +} // namespace experimental +} // namespace rclcpp + +#endif // RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_BASE_HPP_ diff --git a/rclcpp/include/rclcpp/intra_process_setting.hpp b/rclcpp/include/rclcpp/intra_process_setting.hpp index 8e4b44eb64..5207622592 100644 --- a/rclcpp/include/rclcpp/intra_process_setting.hpp +++ b/rclcpp/include/rclcpp/intra_process_setting.hpp @@ -18,12 +18,12 @@ namespace rclcpp { -/// Used as argument in create_publisher and create_subscriber. +/// Used as argument when creating publishers, subscriptions, clients and services. enum class IntraProcessSetting { - /// Explicitly enable intraprocess comm at publisher/subscription level. + /// Explicitly enable intraprocess comm. Enable, - /// Explicitly disable intraprocess comm at publisher/subscription level. + /// Explicitly disable intraprocess comm. Disable, /// Take intraprocess configuration from the node. NodeDefault diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 7ecb67e9b1..99ae1e360d 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -258,6 +258,7 @@ class Node : public std::enable_shared_from_this * \param[in] service_name The topic to service on. * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. + * \param[in] ipc_setting Intra-process communication setting for the client. * \return Shared pointer to the created client. * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ @@ -267,7 +268,8 @@ class Node : public std::enable_shared_from_this create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault); /// Create and return a Client. /** @@ -289,6 +291,7 @@ class Node : public std::enable_shared_from_this * \param[in] callback User-defined callback function. * \param[in] qos_profile rmw_qos_profile_t Quality of service profile for client. * \param[in] group Callback group to call the service. + * \param[in] ipc_setting Intra-process communication setting for the service. * \return Shared pointer to the created service. * \deprecated use rclcpp::QoS instead of rmw_qos_profile_t */ @@ -299,7 +302,8 @@ class Node : public std::enable_shared_from_this const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group = nullptr); + rclcpp::CallbackGroup::SharedPtr group = nullptr, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault); /// Create and return a Service. /** diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index d6b090d4d1..5a32171e07 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -157,7 +157,8 @@ typename Client::SharedPtr Node::create_client( const std::string & service_name, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + rclcpp::IntraProcessSetting ipc_setting) { return rclcpp::create_client( node_base_, @@ -165,7 +166,8 @@ Node::create_client( node_services_, extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), qos_profile, - group); + group, + ipc_setting); } template @@ -191,7 +193,8 @@ Node::create_service( const std::string & service_name, CallbackT && callback, const rmw_qos_profile_t & qos_profile, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::CallbackGroup::SharedPtr group, + rclcpp::IntraProcessSetting ipc_setting) { return rclcpp::create_service( node_base_, @@ -199,7 +202,8 @@ Node::create_service( extend_name_with_sub_namespace(service_name, this->get_sub_namespace()), std::forward(callback), qos_profile, - group); + group, + ipc_setting); } template diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index f474a67192..3f23ce401c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -158,7 +158,7 @@ class Publisher : public PublisherBase (void)options; // If needed, setup intra process communication. - if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + if (rclcpp::detail::resolve_use_intra_process(options_.use_intra_process_comm, *node_base)) { auto context = node_base->get_context(); // Get the intra process manager instance for this context. auto ipm = context->get_sub_context(); diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 3cfc11e9ca..5b1a19aefe 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -37,8 +37,12 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/detail/cpp_callback_trampoline.hpp" +#include "rclcpp/detail/resolve_use_intra_process.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/experimental/intra_process_manager.hpp" +#include "rclcpp/experimental/service_intra_process.hpp" +#include "rclcpp/intra_process_setting.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" @@ -48,16 +52,22 @@ namespace rclcpp { +namespace node_interfaces +{ +class NodeBaseInterface; +} // namespace node_interfaces + class ServiceBase { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase) RCLCPP_PUBLIC - explicit ServiceBase(std::shared_ptr node_handle); + explicit ServiceBase( + std::shared_ptr node_base); RCLCPP_PUBLIC - virtual ~ServiceBase() = default; + virtual ~ServiceBase(); /// Return the name of the service. /** \return The name of the service. */ @@ -162,6 +172,15 @@ class ServiceBase rclcpp::QoS get_request_subscription_actual_qos() const; + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable(); + /// Set a callback to be called when each new request is received. /** * The callback receives a size_t which is the number of requests received @@ -263,8 +282,28 @@ class ServiceBase void set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data); + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implementation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t intra_process_service_id, + IntraProcessManagerWeakPtr weak_ipm); + + std::shared_ptr service_intra_process_; + std::shared_ptr node_handle_; + std::shared_ptr context_; + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_request_callback_ before + // service_handle_, so on destruction the service is + // destroyed first. Otherwise, the rmw service callback + // would point briefly to a destroyed function. + std::function on_new_request_callback_{nullptr}; + // Declare service_handle_ after callback std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; @@ -272,8 +311,10 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex callback_mutex_; - std::function on_new_request_callback_{nullptr}; + std::recursive_mutex ipc_mutex_; + bool use_intra_process_{false}; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t intra_process_service_id_; }; template @@ -304,13 +345,15 @@ class Service * \param[in] service_name Name of the topic to publish to. * \param[in] any_callback User defined callback to call when a client request is received. * \param[in] service_options options for the subscription. + * \param[in] ipc_setting Intra-process communication setting for the service. */ Service( - std::shared_ptr node_handle, + std::shared_ptr node_base, const std::string & service_name, AnyServiceCallback any_callback, - rcl_service_options_t & service_options) - : ServiceBase(node_handle), any_callback_(any_callback), + rcl_service_options_t & service_options, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) + : ServiceBase(node_base), any_callback_(any_callback), srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // rcl does the static memory allocation here @@ -330,7 +373,7 @@ class Service rcl_ret_t ret = rcl_service_init( service_handle_.get(), - node_handle.get(), + node_handle_.get(), srv_type_support_handle_, service_name.c_str(), &service_options); @@ -355,6 +398,11 @@ class Service #ifndef TRACETOOLS_DISABLED any_callback_.register_callback_for_tracing(); #endif + + // Setup intra process if requested. + if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) { + create_intra_process_service(); + } } /// Default constructor. @@ -366,12 +414,14 @@ class Service * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. * \param[in] service_handle service handle. * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] ipc_setting Intra-process communication setting for the service. */ Service( - std::shared_ptr node_handle, + std::shared_ptr node_base, std::shared_ptr service_handle, - AnyServiceCallback any_callback) - : ServiceBase(node_handle), any_callback_(any_callback), + AnyServiceCallback any_callback, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) + : ServiceBase(node_base), any_callback_(any_callback), srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized @@ -390,6 +440,11 @@ class Service #ifndef TRACETOOLS_DISABLED any_callback_.register_callback_for_tracing(); #endif + + // Setup intra process if requested. + if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) { + create_intra_process_service(); + } } /// Default constructor. @@ -401,12 +456,14 @@ class Service * \param[in] node_handle NodeBaseInterface pointer that is used in part of the setup. * \param[in] service_handle service handle. * \param[in] any_callback User defined callback to call when a client request is received. + * \param[in] ipc_setting Intra-process communication setting for the service. */ Service( - std::shared_ptr node_handle, + std::shared_ptr node_base, rcl_service_t * service_handle, - AnyServiceCallback any_callback) - : ServiceBase(node_handle), any_callback_(any_callback), + AnyServiceCallback any_callback, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::NodeDefault) + : ServiceBase(node_base), any_callback_(any_callback), srv_type_support_handle_(rosidl_typesupport_cpp::get_service_type_support_handle()) { // check if service handle was initialized @@ -427,12 +484,28 @@ class Service #ifndef TRACETOOLS_DISABLED any_callback_.register_callback_for_tracing(); #endif + // Setup intra process if requested. + if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) { + create_intra_process_service(); + } } Service() = delete; virtual ~Service() { + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a service."); + return; + } + ipm->remove_service(intra_process_service_id_); } /// Take the next request from the service. @@ -482,11 +555,53 @@ class Service { rcl_ret_t ret = rcl_send_response(get_service_handle().get(), &req_id, &response); + if (ret == RCL_RET_TIMEOUT) { + RCLCPP_WARN( + node_logger_.get_child("rclcpp"), + "failed to send response to %s (timeout): %s", + this->get_service_name(), rcl_get_error_string().str); + rcl_reset_error(); + return; + } if (ret != RCL_RET_OK) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); } } + void + create_intra_process_service() + { + // Check if the QoS is compatible with intra-process. + auto qos_profile = get_request_subscription_actual_qos(); + + if (qos_profile.history() != rclcpp::HistoryPolicy::KeepLast) { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + if (qos_profile.depth() == 0) { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + if (qos_profile.durability() != rclcpp::DurabilityPolicy::Volatile) { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + // Create a ServiceIntraProcess which will be given to the intra-process manager. + using ServiceIntraProcessT = rclcpp::experimental::ServiceIntraProcess; + + service_intra_process_ = std::make_shared( + any_callback_, + context_, + this->get_service_name(), + qos_profile); + + using rclcpp::experimental::IntraProcessManager; + auto ipm = context_->get_sub_context(); + uint64_t intra_process_service_id = ipm->add_intra_process_service(service_intra_process_); + this->setup_intra_process(intra_process_service_id, ipm); + } + /// Configure client introspection. /** * \param[in] clock clock to use to generate introspection timestamps diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index eb1a980dd3..400941a9aa 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -150,7 +150,7 @@ class Subscription : public SubscriptionBase message_memory_strategy_(message_memory_strategy) { // Setup intra process publishing if requested. - if (rclcpp::detail::resolve_use_intra_process(options_, *node_base)) { + if (rclcpp::detail::resolve_use_intra_process(options_.use_intra_process_comm, *node_base)) { using rclcpp::detail::resolve_intra_process_buffer_type; // Check if the QoS is compatible with intra-process. diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index b122e96575..e3b6840da1 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -288,7 +288,7 @@ class SubscriptionBase : public std::enable_shared_from_this using IntraProcessManagerWeakPtr = std::weak_ptr; - /// Implemenation detail. + /// Implementation detail. RCLCPP_PUBLIC void setup_intra_process( @@ -645,6 +645,14 @@ class SubscriptionBase : public std::enable_shared_from_this rclcpp::node_interfaces::NodeBaseInterface * const node_base_; std::shared_ptr node_handle_; + + std::recursive_mutex callback_mutex_; + // It is important to declare on_new_message_callback_ before + // subscription_handle_, so on destruction the subscription is + // destroyed first. Otherwise, the rmw subscription callback + // would point briefly to a destroyed function. + std::function on_new_message_callback_{nullptr}; + // Declare subscription_handle_ after callback std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; rclcpp::Logger node_logger_; @@ -669,9 +677,6 @@ class SubscriptionBase : public std::enable_shared_from_this std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; std::unordered_map> qos_events_in_use_by_wait_set_; - - std::recursive_mutex callback_mutex_; - std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/action_client_intra_process_base.cpp b/rclcpp/src/rclcpp/action_client_intra_process_base.cpp new file mode 100644 index 0000000000..68f9429c09 --- /dev/null +++ b/rclcpp/src/rclcpp/action_client_intra_process_base.cpp @@ -0,0 +1,36 @@ +// Copyright 2022 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/experimental/action_client_intra_process_base.hpp" + +using rclcpp::experimental::ActionClientIntraProcessBase; + +void +ActionClientIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); +} + +const char * +ActionClientIntraProcessBase::get_action_name() const +{ + return action_name_.c_str(); +} + +rclcpp::QoS +ActionClientIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} diff --git a/rclcpp/src/rclcpp/action_server_intra_process_base.cpp b/rclcpp/src/rclcpp/action_server_intra_process_base.cpp new file mode 100644 index 0000000000..94d0c083c3 --- /dev/null +++ b/rclcpp/src/rclcpp/action_server_intra_process_base.cpp @@ -0,0 +1,36 @@ +// Copyright 2022 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/experimental/action_server_intra_process_base.hpp" + +using rclcpp::experimental::ActionServerIntraProcessBase; + +void +ActionServerIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); +} + +const char * +ActionServerIntraProcessBase::get_action_name() const +{ + return action_name_.c_str(); +} + +rclcpp::QoS +ActionServerIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e33db71ce2..be29ba53ed 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -67,6 +67,26 @@ ClientBase::ClientBase( }); } +ClientBase::~ClientBase() +{ + clear_on_new_response_callback(); + // Make sure the client handle is destructed as early as possible and before the node handle + client_handle_.reset(); + + std::lock_guard ipc_lock(ipc_mutex_); + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a client."); + return; + } + ipm->remove_client(intra_process_client_id_); +} + bool ClientBase::take_type_erased_response(void * response_out, rmw_request_id_t & request_header_out) { @@ -246,3 +266,34 @@ ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const vo throw_from_rcl_error(ret, "failed to set the on new response callback for client"); } } + +void +ClientBase::setup_intra_process( + uint64_t intra_process_client_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + std::lock_guard ipc_lock(ipc_mutex_); + weak_ipm_ = weak_ipm; + use_intra_process_ = true; + intra_process_client_id_ = intra_process_client_id; +} + +rclcpp::Waitable::SharedPtr +ClientBase::get_intra_process_waitable() +{ + std::lock_guard ipc_lock(ipc_mutex_); + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "ClientBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the intra-process client from the intra-process manager. + return ipm->get_client_intra_process(intra_process_client_id_); +} diff --git a/rclcpp/src/rclcpp/client_intra_process_base.cpp b/rclcpp/src/rclcpp/client_intra_process_base.cpp new file mode 100644 index 0000000000..b445f32937 --- /dev/null +++ b/rclcpp/src/rclcpp/client_intra_process_base.cpp @@ -0,0 +1,36 @@ +// Copyright 2021 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/experimental/client_intra_process_base.hpp" + +using rclcpp::experimental::ClientIntraProcessBase; + +void +ClientIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); +} + +const char * +ClientIntraProcessBase::get_service_name() const +{ + return service_name_.c_str(); +} + +rclcpp::QoS +ClientIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index 40ae6c030d..d4b4d57b08 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -39,13 +39,6 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( EventHandlerBase::~EventHandlerBase() { - // Since the rmw event listener holds a reference to - // this callback, we need to clear it on destruction of this class. - // This clearing is not needed for other rclcpp entities like pub/subs, since - // they do own the underlying rmw entities, which are destroyed - // on their rclcpp destructors, thus no risk of dangling pointers. - clear_on_ready_callback(); - if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index 094ff21282..64b07c0814 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -50,6 +50,9 @@ EventsExecutor::EventsExecutor( timers_manager_ = std::make_shared(context_, timer_on_ready_cb); + this->current_entities_collection_ = + std::make_shared(); + notify_waitable_ = std::make_shared( [this]() { // This callback is invoked when: @@ -61,6 +64,10 @@ EventsExecutor::EventsExecutor( this->refresh_current_collection_from_callback_groups(); }); + // Make sure that the notify waitable is immediately added to the collection + // to avoid missing events + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); + notify_waitable_->add_guard_condition(interrupt_guard_condition_); notify_waitable_->add_guard_condition(shutdown_guard_condition_); @@ -87,9 +94,6 @@ EventsExecutor::EventsExecutor( this->entities_collector_ = std::make_shared(notify_waitable_); - - this->current_entities_collection_ = - std::make_shared(); } EventsExecutor::~EventsExecutor() @@ -269,10 +273,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) switch (event.type) { case ExecutorEventType::CLIENT_EVENT: { - auto client = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->clients); - + rclcpp::ClientBase::SharedPtr client; + { + std::lock_guard lock(collection_mutex_); + client = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->clients); + } if (client) { for (size_t i = 0; i < event.num_events; i++) { execute_client(client); @@ -283,9 +290,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SUBSCRIPTION_EVENT: { - auto subscription = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->subscriptions); + rclcpp::SubscriptionBase::SharedPtr subscription; + { + std::lock_guard lock(collection_mutex_); + subscription = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->subscriptions); + } if (subscription) { for (size_t i = 0; i < event.num_events; i++) { execute_subscription(subscription); @@ -295,10 +306,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::SERVICE_EVENT: { - auto service = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->services); - + rclcpp::ServiceBase::SharedPtr service; + { + std::lock_guard lock(collection_mutex_); + service = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->services); + } if (service) { for (size_t i = 0; i < event.num_events; i++) { execute_service(service); @@ -315,9 +329,13 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } case ExecutorEventType::WAITABLE_EVENT: { - auto waitable = this->retrieve_entity( - static_cast(event.entity_key), - current_entities_collection_->waitables); + rclcpp::Waitable::SharedPtr waitable; + { + std::lock_guard lock(collection_mutex_); + waitable = this->retrieve_entity( + static_cast(event.entity_key), + current_entities_collection_->waitables); + } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { auto data = waitable->take_data_by_entity_id(event.waitable_data); @@ -382,6 +400,7 @@ EventsExecutor::get_automatically_added_callback_groups_from_nodes() void EventsExecutor::refresh_current_collection_from_callback_groups() { + // Build the new collection this->entities_collector_->update_collections(); auto callback_groups = this->entities_collector_->get_all_callback_groups(); rclcpp::executors::ExecutorEntitiesCollection new_collection; @@ -395,18 +414,11 @@ EventsExecutor::refresh_current_collection_from_callback_groups() // retrieved in the "standard" way. // To do it, we need to add the notify waitable as an entry in both the new and // current collections such that it's neither added or removed. - rclcpp::CallbackGroup::WeakPtr weak_group_ptr; - new_collection.waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + this->add_notify_waitable_to_collection(new_collection.waitables); - this->current_entities_collection_->waitables.insert( - { - this->notify_waitable_.get(), - {this->notify_waitable_, weak_group_ptr} - }); + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + this->add_notify_waitable_to_collection(current_entities_collection_->waitables); this->refresh_current_collection(new_collection); } @@ -415,6 +427,9 @@ void EventsExecutor::refresh_current_collection( const rclcpp::executors::ExecutorEntitiesCollection & new_collection) { + // Acquire lock before modifying the current collection + std::lock_guard lock(collection_mutex_); + current_entities_collection_->timers.update( new_collection.timers, [this](rclcpp::TimerBase::SharedPtr timer) {timers_manager_->add_timer(timer);}, @@ -486,3 +501,16 @@ EventsExecutor::create_waitable_callback(const rclcpp::Waitable * entity_key) }; return callback; } + +void +EventsExecutor::add_notify_waitable_to_collection( + rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection) +{ + // The notify waitable is not associated to any group, so use an invalid one + rclcpp::CallbackGroup::WeakPtr weak_group_ptr; + collection.insert( + { + this->notify_waitable_.get(), + {this->notify_waitable_, weak_group_ptr} + }); +} diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index efce4afeaf..1aab3a2682 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -25,12 +25,6 @@ namespace experimental static std::atomic _next_unique_id {1}; -IntraProcessManager::IntraProcessManager() -{} - -IntraProcessManager::~IntraProcessManager() -{} - uint64_t IntraProcessManager::add_publisher(rclcpp::PublisherBase::SharedPtr publisher) { @@ -82,6 +76,180 @@ IntraProcessManager::add_subscription(SubscriptionIntraProcessBase::SharedPtr su return sub_id; } +uint64_t +IntraProcessManager::add_intra_process_client(ClientIntraProcessBase::SharedPtr client) +{ + std::unique_lock lock(mutex_); + + uint64_t client_id = IntraProcessManager::get_next_unique_id(); + clients_[client_id] = client; + + // adds the client to the matchable service + for (auto & pair : services_) { + auto intra_process_service = pair.second.lock(); + if (!intra_process_service) { + continue; + } + if (can_communicate(client, intra_process_service)) { + uint64_t service_id = pair.first; + clients_to_services_.emplace(client_id, service_id); + intra_process_service->add_intra_process_client(client, client_id); + break; + } + } + + return client_id; +} + +uint64_t +IntraProcessManager::add_intra_process_service(ServiceIntraProcessBase::SharedPtr service) +{ + std::unique_lock lock(mutex_); + + // First check if we have already a service registered with same service name + // Todo: Open issue about this not being enforced with normal services + auto it = services_.begin(); + + while (it != services_.end()) { + auto srv = it->second.lock(); + if (srv) { + if (srv->get_service_name() == service->get_service_name()) { + throw std::runtime_error( + "Can't have multiple services with same service name."); + } + it++; + } else { + it = services_.erase(it); + } + } + + uint64_t service_id = IntraProcessManager::get_next_unique_id(); + services_[service_id] = service; + + // adds the service to the matchable clients + for (auto & pair : clients_) { + auto client = pair.second.lock(); + if (!client) { + continue; + } + if (can_communicate(client, service)) { + uint64_t client_id = pair.first; + clients_to_services_.emplace(client_id, service_id); + + service->add_intra_process_client(client, client_id); + } + } + + return service_id; +} + +uint64_t +IntraProcessManager::add_intra_process_action_client( + ActionClientIntraProcessBase::SharedPtr client) +{ + std::unique_lock lock(mutex_); + + uint64_t client_id = IntraProcessManager::get_next_unique_id(); + action_clients_[client_id] = client; + + // adds the intra-process action client to the matchable action server + for (auto & pair : action_servers_) { + auto server = pair.second.lock(); + if (!server) { + continue; + } + if (can_communicate(client, server)) { + uint64_t server_id = pair.first; + action_clients_to_servers_.emplace(client_id, server_id); + break; + } + } + return client_id; +} + +uint64_t +IntraProcessManager::add_intra_process_action_server( + ActionServerIntraProcessBase::SharedPtr server) +{ + std::unique_lock lock(mutex_); + + // First check if we have already a server registered with same server name + auto it = action_servers_.begin(); + + while (it != action_servers_.end()) { + auto ipc_action_server = it->second.lock(); + if (ipc_action_server) { + if (ipc_action_server->get_action_name() == server->get_action_name()) { + throw std::runtime_error( + "Can't have multiple action servers with same server name."); + } + it++; + } else { + it = action_servers_.erase(it); + } + } + + uint64_t server_id = IntraProcessManager::get_next_unique_id(); + action_servers_[server_id] = server; + + // adds the server to the matchable action_clients + for (auto & pair : action_clients_) { + auto ipc_action_client = pair.second.lock(); + if (!ipc_action_client) { + continue; + } + if (can_communicate(ipc_action_client, server)) { + uint64_t client_id = pair.first; + action_clients_to_servers_.emplace(client_id, server_id); + } + } + return server_id; +} + +// Store an intra-process action client ID along its current +// goal UUID, since later when the server process a request +// it'll use the goal UUID to retrieve the client which asked for +// the result. +void +IntraProcessManager::store_intra_process_action_client_goal_uuid( + uint64_t ipc_action_client_id, + size_t uuid) +{ + std::unique_lock lock(mutex_); + clients_uuid_to_id_[uuid] = ipc_action_client_id; +} + +// Remove an action client goal UUID entry +void +IntraProcessManager::remove_intra_process_action_client_goal_uuid(size_t uuid) +{ + std::unique_lock lock(mutex_); + + auto iter = clients_uuid_to_id_.find(uuid); + + if (iter == clients_uuid_to_id_.end()) { + throw std::runtime_error( + "No ipc action clients stored with this UUID."); + } + + clients_uuid_to_id_.erase(iter); +} + +uint64_t +IntraProcessManager::get_action_client_id_from_goal_uuid(size_t uuid) +{ + std::unique_lock lock(mutex_); + + auto iter = clients_uuid_to_id_.find(uuid); + + if (iter == clients_uuid_to_id_.end()) { + throw std::runtime_error( + "No ipc action clients stored with this UUID."); + } + + return iter->second; +} + void IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { @@ -115,6 +283,70 @@ IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) pub_to_subs_.erase(intra_process_publisher_id); } +void +IntraProcessManager::remove_client(uint64_t intra_process_client_id) +{ + std::unique_lock lock(mutex_); + + clients_.erase(intra_process_client_id); + clients_to_services_.erase(intra_process_client_id); +} + +void +IntraProcessManager::remove_service(uint64_t intra_process_service_id) +{ + std::unique_lock lock(mutex_); + + services_.erase(intra_process_service_id); + + auto it = clients_to_services_.begin(); + + while (it != clients_to_services_.end()) { + if (it->second == intra_process_service_id) { + it = clients_to_services_.erase(it); + } else { + it++; + } + } +} + +void +IntraProcessManager::remove_action_client(uint64_t ipc_action_client_id) +{ + std::unique_lock lock(mutex_); + + action_clients_.erase(ipc_action_client_id); + action_clients_to_servers_.erase(ipc_action_client_id); + + auto it = clients_uuid_to_id_.begin(); + + while (it != clients_uuid_to_id_.end()) { + if (it->second == ipc_action_client_id) { + it = clients_uuid_to_id_.erase(it); + } else { + it++; + } + } +} + +void +IntraProcessManager::remove_action_server(uint64_t ipc_action_server_id) +{ + std::unique_lock lock(mutex_); + + action_servers_.erase(ipc_action_server_id); + + auto it = action_clients_to_servers_.begin(); + + while (it != action_clients_to_servers_.end()) { + if (it->second == ipc_action_server_id) { + it = action_clients_to_servers_.erase(it); + } else { + it++; + } + } +} + bool IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const { @@ -172,6 +404,111 @@ IntraProcessManager::get_subscription_intra_process(uint64_t intra_process_subsc } } +ServiceIntraProcessBase::SharedPtr +IntraProcessManager::get_service_intra_process(uint64_t intra_process_service_id) +{ + std::shared_lock lock(mutex_); + + auto service_it = services_.find(intra_process_service_id); + if (service_it == services_.end()) { + return nullptr; + } else { + auto service = service_it->second.lock(); + if (service) { + return service; + } else { + services_.erase(service_it); + return nullptr; + } + } +} + +ClientIntraProcessBase::SharedPtr +IntraProcessManager::get_client_intra_process(uint64_t intra_process_client_id) +{ + std::shared_lock lock(mutex_); + + auto client_it = clients_.find(intra_process_client_id); + if (client_it == clients_.end()) { + return nullptr; + } else { + auto client = client_it->second.lock(); + if (client) { + return client; + } else { + clients_.erase(client_it); + return nullptr; + } + } +} + +ActionClientIntraProcessBase::SharedPtr +IntraProcessManager::get_action_client_intra_process( + uint64_t intra_process_action_client_id) +{ + std::shared_lock lock(mutex_); + + auto client_it = action_clients_.find(intra_process_action_client_id); + if (client_it == action_clients_.end()) { + return nullptr; + } else { + auto client = client_it->second.lock(); + if (client) { + return client; + } else { + action_clients_.erase(client_it); + return nullptr; + } + } +} + +ActionServerIntraProcessBase::SharedPtr +IntraProcessManager::get_action_server_intra_process( + uint64_t intra_process_action_server_id) +{ + std::shared_lock lock(mutex_); + + auto service_it = action_servers_.find(intra_process_action_server_id); + if (service_it == action_servers_.end()) { + return nullptr; + } else { + auto service = service_it->second.lock(); + if (service) { + return service; + } else { + action_servers_.erase(service_it); + return nullptr; + } + } +} + +bool +IntraProcessManager::service_is_available(uint64_t intra_process_client_id) +{ + std::shared_lock lock(mutex_); + + auto service_it = clients_to_services_.find(intra_process_client_id); + + if (service_it != clients_to_services_.end()) { + // A server matching the client has been found + return true; + } + return false; +} + +bool +IntraProcessManager::action_server_is_available(uint64_t ipc_action_client_id) +{ + std::shared_lock lock(mutex_); + auto action_service_it = action_clients_to_servers_.find(ipc_action_client_id); + + if (action_service_it != action_clients_to_servers_.end()) { + // An action server matching the action client has been found + return true; + } + return false; +} + uint64_t IntraProcessManager::get_next_unique_id() { @@ -225,5 +562,45 @@ IntraProcessManager::can_communicate( return true; } +bool +IntraProcessManager::can_communicate( + ClientIntraProcessBase::SharedPtr client, + ServiceIntraProcessBase::SharedPtr service) const +{ + // client and service must be under the same name + if (strcmp(client->get_service_name(), service->get_service_name()) != 0) { + return false; + } + + auto check_result = rclcpp::qos_check_compatible( + client->get_actual_qos(), service->get_actual_qos()); + + if (check_result.compatibility == rclcpp::QoSCompatibility::Error) { + return false; + } + + return true; +} + +bool +IntraProcessManager::can_communicate( + ActionClientIntraProcessBase::SharedPtr client, + ActionServerIntraProcessBase::SharedPtr service) const +{ + // client and service must be under the same name + if (strcmp(client->get_action_name(), service->get_action_name()) != 0) { + return false; + } + + auto check_result = rclcpp::qos_check_compatible( + client->get_actual_qos(), service->get_actual_qos()); + + if (check_result.compatibility == rclcpp::QoSCompatibility::Error) { + return false; + } + + return true; +} + } // namespace experimental } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp index e9c4a5266e..f3912c2cda 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_services.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_services.cpp @@ -41,6 +41,12 @@ NodeServices::add_service( group->add_service(service_base_ptr); + auto service_intra_process_waitable = service_base_ptr->get_intra_process_waitable(); + if (nullptr != service_intra_process_waitable) { + // Add to the callback group to be notified about intra-process msgs. + group->add_waitable(service_intra_process_waitable); + } + // Notify the executor that a new service was created using the parent Node. try { node_base_->trigger_notify_guard_condition(); @@ -67,6 +73,12 @@ NodeServices::add_client( group->add_client(client_base_ptr); + auto client_intra_process_waitable = client_base_ptr->get_intra_process_waitable(); + if (nullptr != client_intra_process_waitable) { + // Add to the callback group to be notified about intra-process msgs. + group->add_waitable(client_intra_process_waitable); + } + // Notify the executor that a new client was created using the parent Node. try { node_base_->trigger_notify_guard_condition(); diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 9c246e4b6b..4ef8474f71 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -21,6 +21,7 @@ #include #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/qos.hpp" #include "rmw/error_handling.h" @@ -28,11 +29,30 @@ using rclcpp::ServiceBase; -ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle), - node_logger_(rclcpp::get_node_logger(node_handle_.get())) +ServiceBase::ServiceBase( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) +: node_handle_(node_base->get_shared_rcl_node_handle()), + context_(node_base->get_context()), + node_logger_(rclcpp::get_node_logger(node_base->get_shared_rcl_node_handle().get())) {} +ServiceBase::~ServiceBase() +{ + clear_on_new_request_callback(); + + std::lock_guard ipc_lock(ipc_mutex_); + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than a service."); + return; + } + ipm->remove_service(intra_process_service_id_); +} bool ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) @@ -123,6 +143,37 @@ ServiceBase::get_request_subscription_actual_qos() const return request_subscription_qos; } +void +ServiceBase::setup_intra_process( + uint64_t intra_process_service_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + std::lock_guard ipc_lock(ipc_mutex_); + intra_process_service_id_ = intra_process_service_id; + weak_ipm_ = weak_ipm; + use_intra_process_ = true; +} + +rclcpp::Waitable::SharedPtr +ServiceBase::get_intra_process_waitable() +{ + std::lock_guard ipc_lock(ipc_mutex_); + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "ServiceBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the intra-process service from the intra-process manager. + return ipm->get_service_intra_process(intra_process_service_id_); +} + void ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) { diff --git a/rclcpp/src/rclcpp/service_intra_process_base.cpp b/rclcpp/src/rclcpp/service_intra_process_base.cpp new file mode 100644 index 0000000000..2ccb38779c --- /dev/null +++ b/rclcpp/src/rclcpp/service_intra_process_base.cpp @@ -0,0 +1,45 @@ +// Copyright 2021 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 "rclcpp/detail/add_guard_condition_to_rcl_wait_set.hpp" +#include "rclcpp/experimental/service_intra_process_base.hpp" + +using rclcpp::experimental::ServiceIntraProcessBase; + +void +ServiceIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); +} + +const char * +ServiceIntraProcessBase::get_service_name() const +{ + return service_name_.c_str(); +} + +rclcpp::QoS +ServiceIntraProcessBase::get_actual_qos() const +{ + return qos_profile_; +} + +void +ServiceIntraProcessBase::add_intra_process_client( + rclcpp::experimental::ClientIntraProcessBase::SharedPtr client, + uint64_t client_id) +{ + std::unique_lock lock(reentrant_mutex_); + clients_[client_id] = client; +} diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 7f3b3b9536..ef446dc59f 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -242,6 +242,7 @@ class TimeSource::NodeState final rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface, rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface) { + std::lock_guard guard(node_base_lock_); node_base_ = node_base_interface; node_topics_ = node_topics_interface; node_graph_ = node_graph_interface; @@ -286,17 +287,14 @@ class TimeSource::NodeState final parameter_subscription_ = rclcpp::AsyncParametersClient::on_parameter_event( node_topics_, [this](std::shared_ptr event) { - if (node_base_ != nullptr) { - this->on_parameter_event(event); - } - // Do nothing if node_base_ is nullptr because it means the TimeSource is now - // without an attached node + this->on_parameter_event(event); }); } // Detach the attached node void detachNode() { + std::lock_guard guard(node_base_lock_); // destroy_clock_sub() *must* be first here, to ensure that the executor // can't possibly call any of the callbacks as we are cleaning up. destroy_clock_sub(); @@ -333,6 +331,7 @@ class TimeSource::NodeState final std::thread clock_executor_thread_; // Preserve the node reference + std::mutex node_base_lock_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_{nullptr}; rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_{nullptr}; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_{nullptr}; @@ -470,6 +469,14 @@ class TimeSource::NodeState final // Callback for parameter updates void on_parameter_event(std::shared_ptr event) { + std::lock_guard guard(node_base_lock_); + + if (node_base_ == nullptr) { + // Do nothing if node_base_ is nullptr because it means the TimeSource is now + // without an attached node + return; + } + // Filter out events on 'use_sim_time' parameter instances in other nodes. if (event->node != node_base_->get_fully_qualified_name()) { return; diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 38b6ddf870..e9aad61685 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -20,12 +20,14 @@ #include #include +#include #include #include #include #include #include #include +#include #include "rcl/error_handling.h" #include "rcl/time.h" @@ -43,18 +45,10 @@ template class TestExecutors : public ::testing::Test { public: - static void SetUpTestCase() + void SetUp() { rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - void SetUp() - { const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); std::stringstream test_name; test_name << test_info->test_case_name() << "_" << test_info->name(); @@ -75,6 +69,8 @@ class TestExecutors : public ::testing::Test publisher.reset(); subscription.reset(); node.reset(); + + rclcpp::shutdown(); } rclcpp::Node::SharedPtr node; @@ -729,6 +725,131 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureCompleteInterrupted) spinner.join(); } +// This test verifies that the add_node operation is robust wrt race conditions. +// It's mostly meant to prevent regressions in the events-executor, but the operation should be +// thread-safe in all executor implementations. +// The initial implementation of the events-executor contained a bug where the executor +// would end up in an inconsistent state and stop processing interrupt/shutdown notifications. +// Manually adding a node to the executor results in a) producing a notify waitable event +// and b) refreshing the executor collections. +// The inconsistent state would happen if the event was processed before the collections were +// finished to be refreshed: the executor would pick up the event but be unable to process it. +// This would leave the `notify_waitable_event_pushed_` flag to true, preventing additional +// notify waitable events to be pushed. +// The behavior is observable only under heavy load, so this test spawns several worker +// threads. Due to the nature of the bug, this test may still succeed even if the +// bug is present. However repeated runs will show its flakiness nature and indicate +// an eventual regression. +TYPED_TEST(TestExecutors, testRaceConditionAddNode) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + // Spawn some threads to do some heavy work + std::atomic should_cancel = false; + std::vector stress_threads; + for (size_t i = 0; i < 5 * std::thread::hardware_concurrency(); i++) { + stress_threads.emplace_back( + [&should_cancel, i]() { + // This is just some arbitrary heavy work + volatile size_t total = 0; + for (size_t k = 0; k < 549528914167; k++) { + if (should_cancel) { + break; + } + total += k * (i + 42); + } + }); + } + + // Create an executor + auto executor = std::make_shared(); + // Start spinning + auto executor_thread = std::thread( + [executor]() { + executor->spin(); + }); + // Add a node to the executor + executor->add_node(this->node); + + // Cancel the executor (make sure that it's already spinning first) + while (!executor->is_spinning() && rclcpp::ok()) { + continue; + } + executor->cancel(); + + // Try to join the thread after cancelling the executor + // This is the "test". We want to make sure that we can still cancel the executor + // regardless of the presence of race conditions + executor_thread.join(); + + // The test is now completed: we can join the stress threads + should_cancel = true; + for (auto & t : stress_threads) { + t.join(); + } +} + +// This test verifies the thread-safety of adding and removing a node +// while the executor is spinning and events are ready. +// This test does not contain expectations, but rather it verifies that +// we can run a "stressful routine" without crashing. +TYPED_TEST(TestExecutors, stressAddRemoveNode) +{ + using ExecutorType = TypeParam; + // rmw_connextdds doesn't support events-executor + if ( + std::is_same() && + std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0) + { + GTEST_SKIP(); + } + + ExecutorType executor; + + // A timer that is "always" ready (the timer callback doesn't do anything) + auto timer = this->node->create_wall_timer(std::chrono::nanoseconds(1), []() {}); + + // This thread spins the executor until it's cancelled + std::thread spinner_thread([&]() { + executor.spin(); + }); + + // This thread publishes data in a busy loop (the node has a subscription) + std::thread publisher_thread1([&]() { + for (size_t i = 0; i < 100000; i++) { + this->publisher->publish(test_msgs::msg::Empty()); + } + }); + std::thread publisher_thread2([&]() { + for (size_t i = 0; i < 100000; i++) { + this->publisher->publish(test_msgs::msg::Empty()); + } + }); + + // This thread adds/remove the node that contains the entities in a busy loop + std::thread add_remove_thread([&]() { + for (size_t i = 0; i < 100000; i++) { + executor.add_node(this->node); + executor.remove_node(this->node); + } + }); + + // Wait for the threads that do real work to finish + publisher_thread1.join(); + publisher_thread2.join(); + add_remove_thread.join(); + + executor.cancel(); + spinner_thread.join(); +} + // Check spin_until_future_complete with node base pointer (instantiates its own executor) TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index ea55a1aac2..9a6353e7d6 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -30,7 +30,7 @@ class TestService : public rclcpp::ServiceBase { public: explicit TestService(rclcpp::Node * node) - : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} + : rclcpp::ServiceBase(node->get_node_base_interface()) {} std::shared_ptr create_request() override {return nullptr;} std::shared_ptr create_request_header() override {return nullptr;} @@ -41,7 +41,8 @@ class TestClient : public rclcpp::ClientBase { public: explicit TestClient(rclcpp::Node * node) - : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} + : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) + {} std::shared_ptr create_response() override {return nullptr;} std::shared_ptr create_request_header() override {return nullptr;} diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 9070e1caa9..b959c0ea41 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -133,7 +133,8 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_services_interface(), "service", rmw_qos_profile_services_default, - nullptr); + nullptr, + rclcpp::IntraProcessSetting::Disable); } { ASSERT_THROW( @@ -144,7 +145,8 @@ TEST_F(TestClient, construction_with_free_function) { node->get_node_services_interface(), "invalid_?service", rmw_qos_profile_services_default, - nullptr); + nullptr, + rclcpp::IntraProcessSetting::Disable); }, rclcpp::exceptions::InvalidServiceNameError); } { diff --git a/rclcpp/test/rclcpp/test_externally_defined_services.cpp b/rclcpp/test/rclcpp/test_externally_defined_services.cpp index 563f7f6ec3..3938c847c7 100644 --- a/rclcpp/test/rclcpp/test_externally_defined_services.cpp +++ b/rclcpp/test/rclcpp/test_externally_defined_services.cpp @@ -66,7 +66,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_uninitialized) { // expect fail try { rclcpp::Service( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + node_handle->get_node_base_interface(), &service_handle, cb); } catch (const std::runtime_error &) { SUCCEED(); @@ -97,7 +97,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_initialized) { try { rclcpp::Service( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + node_handle->get_node_base_interface(), &service_handle, cb); } catch (const std::runtime_error &) { FAIL(); @@ -137,7 +137,7 @@ TEST_F(TestExternallyDefinedServices, extern_defined_destructor) { { // Call constructor rclcpp::Service srv_cpp( - node_handle->get_node_base_interface()->get_shared_rcl_node_handle(), + node_handle->get_node_base_interface(), &service_handle, cb); // Call destructor } diff --git a/rclcpp/test/rclcpp/test_intra_process_manager.cpp b/rclcpp/test/rclcpp/test_intra_process_manager.cpp index 45d916b004..345b0cab9d 100644 --- a/rclcpp/test/rclcpp/test_intra_process_manager.cpp +++ b/rclcpp/test/rclcpp/test_intra_process_manager.cpp @@ -15,9 +15,12 @@ #include #include +#include #include #include +#include #include +#include // NOLINT #include #define RCLCPP_BUILDING_LIBRARY 1 @@ -304,6 +307,144 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< } }; +class ClientIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ClientIntraProcessBase) + + const char * + get_service_name() const + { + return nullptr; + } + + QoS get_actual_qos() const + { + QoS qos(0); + return qos; + } +}; + +template +class ClientIntraProcess : public ClientIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ClientIntraProcess) +}; + +class ServiceIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ServiceIntraProcessBase) + + void + add_intra_process_client( + rclcpp::experimental::mock::ClientIntraProcessBase::SharedPtr client, + uint64_t client_id) + { + (void)client; + (void)client_id; + } + + const char * + get_service_name() const + { + return nullptr; + } + + QoS get_actual_qos() const + { + QoS qos(0); + return qos; + } +}; + +template +class ServiceIntraProcess : public ServiceIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ServiceIntraProcess) + + using SharedRequest = typename ServiceT::Request::SharedPtr; + using SharedResponse = typename ServiceT::Response::SharedPtr; + + using Promise = std::promise; + using PromiseWithRequest = std::promise>; + + using SharedFuture = std::shared_future; + using SharedFutureWithRequest = std::shared_future>; + + using CallbackType = std::function; + using CallbackWithRequestType = std::function; + + using CallbackTypeValueVariant = std::tuple; + using CallbackWithRequestTypeValueVariant = std::tuple< + CallbackWithRequestType, SharedRequest, SharedFutureWithRequest, PromiseWithRequest>; + + using CallbackInfoVariant = std::variant< + std::promise, + CallbackTypeValueVariant, + CallbackWithRequestTypeValueVariant>; + + using RequestCallbackPair = std::pair; + using ClientIDtoRequest = std::pair; + + void + store_intra_process_request( + uint64_t intra_process_client_id, + RequestCallbackPair request) + { + (void)intra_process_client_id; + (void)request; + } +}; + +class ActionClientIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ActionClientIntraProcessBase) + + const char * + get_action_name() const + { + return nullptr; + } + + QoS get_actual_qos() const + { + QoS qos(0); + return qos; + } +}; + +template +class ActionClientIntraProcess : public ActionClientIntraProcessBase +{ +}; + +class ActionServerIntraProcessBase +{ +public: + RCLCPP_SMART_PTR_ALIASES_ONLY(ActionServerIntraProcessBase) + + const char * + get_action_name() const + { + return nullptr; + } + + QoS get_actual_qos() const + { + QoS qos(0); + return qos; + } +}; + +template +class ActionServerIntraProcess : public ActionServerIntraProcessBase +{ +}; + } // namespace mock } // namespace experimental } // namespace rclcpp @@ -314,6 +455,14 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BUFFER_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__SERVICE_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__CLIENT_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_CLIENT_INTRA_PROCESS_BASE_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_HPP_ +#define RCLCPP__EXPERIMENTAL__ACTION_SERVER_INTRA_PROCESS_BASE_HPP_ // Force ipm to use our mock publisher class. #define Publisher mock::Publisher #define PublisherBase mock::PublisherBase @@ -321,12 +470,28 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBuffer< #define SubscriptionIntraProcessBase mock::SubscriptionIntraProcessBase #define SubscriptionIntraProcessBuffer mock::SubscriptionIntraProcessBuffer #define SubscriptionIntraProcess mock::SubscriptionIntraProcess +#define ServiceIntraProcessBase mock::ServiceIntraProcessBase +#define ServiceIntraProcess mock::ServiceIntraProcess +#define ClientIntraProcessBase mock::ClientIntraProcessBase +#define ClientIntraProcess mock::ClientIntraProcess +#define ActionClientIntraProcessBase mock::ActionClientIntraProcessBase +#define ActionClientIntraProcess mock::ActionClientIntraProcess +#define ActionServerIntraProcessBase mock::ActionServerIntraProcessBase +#define ActionServerIntraProcess mock::ActionServerIntraProcess #include "../src/rclcpp/intra_process_manager.cpp" // NOLINT #undef Publisher #undef PublisherBase #undef IntraProcessBuffer #undef SubscriptionIntraProcessBase #undef SubscriptionIntraProcess +#undef ServiceIntraProcessBase +#undef ServiceIntraProcess +#undef ClientIntraProcessBase +#undef ClientIntraProcess +#undef ActionClientIntraProcessBase +#undef ActionClientIntraProcess +#undef ActionServerIntraProcessBase +#undef ActionServerIntraProcess using ::testing::_; using ::testing::UnorderedElementsAre; diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index a3c361cfde..b9ab010e0f 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -19,6 +19,7 @@ #include #include "rclcpp/exceptions.hpp" +#include "rclcpp/intra_process_setting.hpp" #include "rclcpp/rclcpp.hpp" #include "../mocking_utils/patch.hpp" @@ -165,18 +166,28 @@ TEST_F(TestService, basic_public_getters) { rcl_service_options_t service_options = rcl_service_get_default_options(); const rosidl_service_type_support_t * ts = rosidl_typesupport_cpp::get_service_type_support_handle(); + auto node_base_interface = node_handle_int->get_node_base_interface(); rcl_ret_t ret = rcl_service_init( &service_handle, - node_handle_int->get_node_base_interface()->get_rcl_node_handle(), + node_base_interface->get_rcl_node_handle(), ts, "base_node_service", &service_options); if (ret != RCL_RET_OK) { FAIL(); return; } rclcpp::AnyServiceCallback cb; + + + rclcpp::IntraProcessSetting ipc_setting; + if (node_base_interface->get_use_intra_process_default()) { + ipc_setting = rclcpp::IntraProcessSetting::Enable; + } else { + ipc_setting = rclcpp::IntraProcessSetting::Disable; + } + const rclcpp::Service base( - node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(), - &service_handle, cb); + node_handle_int->get_node_base_interface(), + &service_handle, cb, ipc_setting); // Use get_service_handle specific to const service std::shared_ptr const_service_handle = base.get_service_handle(); EXPECT_NE(nullptr, const_service_handle); diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a9bf2a5008..d3cfa853e5 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_ACTION__CLIENT_HPP_ #define RCLCPP_ACTION__CLIENT_HPP_ + #include #include #include @@ -28,6 +29,11 @@ #include "rcl/event_callback.h" +// Check if I need all of following +#include +#include +#include + #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" @@ -180,6 +186,16 @@ class ClientBase : public rclcpp::Waitable // End Waitables API // ----------------- + // Do I need the following? + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable(); + protected: RCLCPP_ACTION_PUBLIC ClientBase( @@ -292,6 +308,16 @@ class ClientBase : public rclcpp::Waitable void handle_status_message(std::shared_ptr message) = 0; + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implementation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t ipc_action_client_id, + IntraProcessManagerWeakPtr weak_ipm); + // End API for communication between ClientBase and Client<> // --------------------------------------------------------- @@ -309,6 +335,12 @@ class ClientBase : public rclcpp::Waitable // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; + // Intra-process action client data fields + std::recursive_mutex ipc_mutex_; + bool use_intra_process_{false}; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t ipc_action_client_id_; + private: std::unique_ptr pimpl_; @@ -394,13 +426,17 @@ class Client : public ClientBase rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, const std::string & action_name, - const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options() - ) + const rcl_action_client_options_t & client_options = rcl_action_client_get_default_options(), + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::Disable) : ClientBase( node_base, node_graph, node_logging, action_name, rosidl_typesupport_cpp::get_action_type_support_handle(), client_options) { + // Setup intra process if requested. + if (rclcpp::detail::resolve_use_intra_process(ipc_setting, *node_base)) { + create_intra_process_action_client(node_base, action_name, client_options); + } } /// Send an action goal and asynchronously get the result. @@ -428,8 +464,10 @@ class Client : public ClientBase auto goal_request = std::make_shared(); goal_request->goal_id.uuid = this->generate_goal_id(); goal_request->goal = goal; - this->send_goal_request( - std::static_pointer_cast(goal_request), + + // The callback to be called when server accepts the goal, using the server + // response as argument. + auto callback = [this, goal_request, options, promise](std::shared_ptr response) mutable { using GoalResponse = typename ActionT::Impl::SendGoalService::Response; @@ -459,7 +497,38 @@ class Client : public ClientBase if (options.result_callback) { this->make_result_aware(goal_handle); } - }); + }; + + bool intra_process_send_done = false; + + std::lock_guard lock(ipc_mutex_); + + if (use_intra_process_) { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process send goal called after destruction of intra process manager"); + } + bool intra_process_server_available = ipm->action_server_is_available(ipc_action_client_id_); + + // Check if there's an intra-process action server available matching this client. + // If there's not, we fall back into inter-process communication, since + // the server might be available in another process or was configured to not use IPC. + if (intra_process_server_available) { + ipm->intra_process_action_send_goal_request( + ipc_action_client_id_, + std::move(goal_request), + callback); + intra_process_send_done = true; + } + } + + if (!intra_process_send_done) { + // Send inter-process goal request + this->send_goal_request( + std::static_pointer_cast(goal_request), + callback); + } // TODO(jacobperron): Encapsulate into it's own function and // consider exposing an option to disable this cleanup @@ -599,6 +668,19 @@ class Client : public ClientBase } it = goal_handles_.erase(it); } + + if (!use_intra_process_) { + return; + } + auto ipm = weak_ipm_.lock(); + if (!ipm) { + // TODO(ivanpauno): should this raise an error? + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Intra process manager died before than an action client."); + return; + } + ipm->remove_action_client(ipc_action_client_id_); } private: @@ -709,23 +791,57 @@ class Client : public ClientBase using GoalResultRequest = typename ActionT::Impl::GetResultService::Request; auto goal_result_request = std::make_shared(); goal_result_request->goal_id.uuid = goal_handle->get_goal_id(); + + // The client callback to be called when server calculates the result, using the server + // response as argument. + auto callback = + [goal_handle, this](std::shared_ptr response) mutable + { + // Wrap the response in a struct with the fields a user cares about + WrappedResult wrapped_result; + using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; + auto result_response = std::static_pointer_cast(response); + wrapped_result.result = std::make_shared(); + *wrapped_result.result = result_response->result; + wrapped_result.goal_id = goal_handle->get_goal_id(); + wrapped_result.code = static_cast(result_response->status); + goal_handle->set_result(wrapped_result); + std::lock_guard lock(goal_handles_mutex_); + goal_handles_.erase(goal_handle->get_goal_id()); + }; + try { - this->send_result_request( - std::static_pointer_cast(goal_result_request), - [goal_handle, this](std::shared_ptr response) mutable - { - // Wrap the response in a struct with the fields a user cares about - WrappedResult wrapped_result; - using GoalResultResponse = typename ActionT::Impl::GetResultService::Response; - auto result_response = std::static_pointer_cast(response); - wrapped_result.result = std::make_shared(); - *wrapped_result.result = result_response->result; - wrapped_result.goal_id = goal_handle->get_goal_id(); - wrapped_result.code = static_cast(result_response->status); - goal_handle->set_result(wrapped_result); - std::lock_guard lock(goal_handles_mutex_); - goal_handles_.erase(goal_handle->get_goal_id()); - }); + bool intra_process_send_done = false; + + std::lock_guard lock(ipc_mutex_); + + if (use_intra_process_) { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process send result called after destruction of intra process manager"); + } + bool intra_process_server_available = + ipm->action_server_is_available(ipc_action_client_id_); + + // Check if there's an intra-process action server available matching this client. + // If there's not, we fall back into inter-process communication, since + // the server might be available in another process or was configured to not use IPC. + if (intra_process_server_available) { + ipm->intra_process_action_send_result_request( + ipc_action_client_id_, + std::move(goal_result_request), + callback); + intra_process_send_done = true; + } + } + + if (!intra_process_send_done) { + // Send inter-process result request + this->send_result_request( + std::static_pointer_cast(goal_result_request), + callback); + } } catch (rclcpp::exceptions::RCLError & ex) { // This will cause an exception when the user tries to access the result goal_handle->invalidate(exceptions::UnawareGoalHandleError(ex.message)); @@ -741,8 +857,8 @@ class Client : public ClientBase // Put promise in the heap to move it around. auto promise = std::make_shared>(); std::shared_future future(promise->get_future()); - this->send_cancel_request( - std::static_pointer_cast(cancel_request), + + auto callback = [cancel_callback, promise](std::shared_ptr response) mutable { auto cancel_response = std::static_pointer_cast(response); @@ -750,12 +866,107 @@ class Client : public ClientBase if (cancel_callback) { cancel_callback(cancel_response); } - }); + }; + + bool intra_process_send_done = false; + + std::lock_guard lock(ipc_mutex_); + + if (use_intra_process_) { + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process send goal called after destruction of intra process manager"); + } + bool intra_process_server_available = ipm->action_server_is_available(ipc_action_client_id_); + + // Check if there's an intra-process action server available matching this client. + // If there's not, we fall back into inter-process communication, since + // the server might be available in another process or was configured to not use IPC. + if (intra_process_server_available) { + ipm->intra_process_action_send_cancel_request( + ipc_action_client_id_, + std::move(cancel_request), + callback); + intra_process_send_done = true; + } + } + + if (!intra_process_send_done) { + this->send_cancel_request( + std::static_pointer_cast(cancel_request), + callback); + } return future; } + void + create_intra_process_action_client( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const std::string & action_name, + const rcl_action_client_options_t & options) + { + auto keep_last = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + if (options.goal_service_qos.history != keep_last || + options.result_service_qos.history != keep_last || + options.cancel_service_qos.history != keep_last || + options.feedback_topic_qos.history != keep_last || + options.status_topic_qos.history != keep_last) + { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + + if (options.goal_service_qos.depth == 0 || + options.result_service_qos.depth == 0 || + options.cancel_service_qos.depth == 0 || + options.feedback_topic_qos.depth == 0 || + options.status_topic_qos.depth == 0) + { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + + auto durability_vol = RMW_QOS_POLICY_DURABILITY_VOLATILE; + if (options.goal_service_qos.durability != durability_vol || + options.result_service_qos.durability != durability_vol || + options.cancel_service_qos.durability != durability_vol || + options.feedback_topic_qos.durability != durability_vol || + options.status_topic_qos.durability != durability_vol) + { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + rcl_action_client_depth_t qos_history; + qos_history.goal_service_depth = options.goal_service_qos.history; + qos_history.result_service_depth = options.result_service_qos.history; + qos_history.cancel_service_depth = options.cancel_service_qos.history; + qos_history.feedback_topic_depth = options.feedback_topic_qos.history; + qos_history.status_topic_depth = options.status_topic_qos.history; + + // Create a ActionClientIntraProcess which will be given + // to the intra-process manager. + auto context = node_base->get_context(); + ipc_action_client_ = std::make_shared( + context, + action_name, + qos_history, + std::bind(&Client::handle_status_message, this, std::placeholders::_1), + std::bind(&Client::handle_feedback_message, this, std::placeholders::_1)); + + // Add it to the intra process manager. + using rclcpp::experimental::IntraProcessManager; + auto ipm = context->get_sub_context(); + uint64_t ipc_action_client_id = ipm->add_intra_process_action_client(ipc_action_client_); + this->setup_intra_process(ipc_action_client_id, ipm); + } + std::map goal_handles_; std::mutex goal_handles_mutex_; + + using ActionClientIntraProcessT = rclcpp::experimental::ActionClientIntraProcess; + std::shared_ptr ipc_action_client_; }; } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/create_client.hpp b/rclcpp_action/include/rclcpp_action/create_client.hpp index f594bca78d..4204a85681 100644 --- a/rclcpp_action/include/rclcpp_action/create_client.hpp +++ b/rclcpp_action/include/rclcpp_action/create_client.hpp @@ -48,7 +48,8 @@ create_client( rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface, const std::string & name, rclcpp::CallbackGroup::SharedPtr group = nullptr, - const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::Disable) { std::weak_ptr weak_node = node_waitables_interface; @@ -85,10 +86,15 @@ create_client( node_graph_interface, node_logging_interface, name, - options), + options, + ipc_setting), deleter); node_waitables_interface->add_waitable(action_client, group); + + auto intra_process_action_client = action_client->get_intra_process_waitable(); + node_waitables_interface->add_waitable(intra_process_action_client, group); + return action_client; } @@ -106,7 +112,8 @@ create_client( NodeT node, const std::string & name, rclcpp::CallbackGroup::SharedPtr group = nullptr, - const rcl_action_client_options_t & options = rcl_action_client_get_default_options()) + const rcl_action_client_options_t & options = rcl_action_client_get_default_options(), + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::Disable) { return rclcpp_action::create_client( node->get_node_base_interface(), @@ -115,7 +122,8 @@ create_client( node->get_node_waitables_interface(), name, group, - options); + options, + ipc_setting); } } // namespace rclcpp_action diff --git a/rclcpp_action/include/rclcpp_action/create_server.hpp b/rclcpp_action/include/rclcpp_action/create_server.hpp index 3aa0e4dea7..9fba2ec7bb 100644 --- a/rclcpp_action/include/rclcpp_action/create_server.hpp +++ b/rclcpp_action/include/rclcpp_action/create_server.hpp @@ -65,7 +65,8 @@ create_server( typename Server::CancelCallback handle_cancel, typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::Disable) { std::weak_ptr weak_node = node_waitables_interface; @@ -104,9 +105,14 @@ create_server( options, handle_goal, handle_cancel, - handle_accepted), deleter); + handle_accepted, + ipc_setting), deleter); node_waitables_interface->add_waitable(action_server, group); + + auto intra_process_action_server = action_server->get_intra_process_waitable(); + node_waitables_interface->add_waitable(intra_process_action_server, group); + return action_server; } @@ -136,7 +142,8 @@ create_server( typename Server::CancelCallback handle_cancel, typename Server::AcceptedCallback handle_accepted, const rcl_action_server_options_t & options = rcl_action_server_get_default_options(), - rclcpp::CallbackGroup::SharedPtr group = nullptr) + rclcpp::CallbackGroup::SharedPtr group = nullptr, + rclcpp::IntraProcessSetting ipc_setting = rclcpp::IntraProcessSetting::Disable) { return create_server( node->get_node_base_interface(), @@ -148,7 +155,8 @@ create_server( handle_cancel, handle_accepted, options, - group); + group, + ipc_setting); } } // namespace rclcpp_action #endif // RCLCPP_ACTION__CREATE_SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 892de5743b..c0a5008ccf 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -22,6 +22,12 @@ #include #include +// Check what I need +#include +#include +#include +#include + #include "rcl/event_callback.h" #include "rcl_action/action_server.h" #include "rosidl_runtime_c/action_type_support_struct.h" @@ -174,6 +180,25 @@ class ServerBase : public rclcpp::Waitable void clear_on_ready_callback() override; + using IntraProcessManagerWeakPtr = + std::weak_ptr; + + /// Implementation detail. + RCLCPP_PUBLIC + void + setup_intra_process( + uint64_t ipc_actionserver_id, + IntraProcessManagerWeakPtr weak_ipm); + + /// Return the waitable for intra-process + /** + * \return the waitable sharedpointer for intra-process, or nullptr if intra-process is not setup. + * \throws std::runtime_error if the intra process manager is destroyed + */ + RCLCPP_PUBLIC + rclcpp::Waitable::SharedPtr + get_intra_process_waitable(); + // End Waitables API // ----------------- @@ -251,6 +276,11 @@ class ServerBase : public rclcpp::Waitable std::shared_ptr create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0; + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + get_status_array(); + /// \internal RCLCPP_ACTION_PUBLIC void @@ -271,9 +301,33 @@ class ServerBase : public rclcpp::Waitable void publish_feedback(std::shared_ptr feedback_msg); + /// Temporary workaround + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + get_rcl_action_goal_handle( + rcl_action_goal_info_t goal_info, + GoalUUID uuid); + + /// Addition + /// \internal + RCLCPP_ACTION_PUBLIC + std::shared_ptr + process_cancel_request(rcl_action_cancel_request_t & cancel_request); + + RCLCPP_ACTION_PUBLIC + std::shared_ptr + get_result_response(GoalUUID uuid); + // End API for communication between ServerBase and Server<> // --------------------------------------------------------- + // Intra-process action server data fields + std::recursive_mutex ipc_mutex_; + bool use_intra_process_{false}; + IntraProcessManagerWeakPtr weak_ipm_; + uint64_t ipc_action_server_id_; + private: /// Handle a request to add a new goal to the server /// \internal @@ -351,6 +405,26 @@ class Server : public ServerBase, public std::enable_shared_from_this>)>; + using ResponseCallback = std::function response)>; + + using GoalRequest = typename ActionT::Impl::SendGoalService::Request; + using GoalRequestSharedPtr = typename std::shared_ptr; + using GoalRequestDataPair = typename std::pair; + using GoalRequestDataPairSharedPtr = typename std::shared_ptr; + + using ResultRequest = typename ActionT::Impl::GetResultService::Request; + using ResultRequestSharedPtr = typename std::shared_ptr; + using ResultRequestDataPair = typename std::pair; + using ResultRequestDataPairSharedPtr = typename std::shared_ptr; + + using CancelRequest = typename ActionT::Impl::CancelGoalService::Request; + using CancelRequestSharedPtr = typename std::shared_ptr; + using CancelRequestDataPair = typename std::pair; + using CancelRequestDataPairSharedPtr = typename std::shared_ptr; + + using ResultResponse = typename ActionT::Impl::GetResultService::Response; + using ResultResponseSharedPtr = typename std::shared_ptr; + /// Construct an action server. /** * This constructs an action server, but it will not work until it has been added to a node. @@ -385,8 +459,8 @@ class Server : public ServerBase, public std::enable_shared_from_thisremove_action_server(ipc_action_server_id_); + } protected: - // ----------------------------------------------------- - // API for communication between ServerBase and Server<> - - /// \internal - std::pair> - call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override + // Intra-process version of execute_goal_request_received_ + // Missing: Deep comparison of functionality betwen IPC on/off + void + ipc_execute_goal_request_received(GoalRequestDataPairSharedPtr data) { - auto request = std::static_pointer_cast< - typename ActionT::Impl::SendGoalService::Request>(message); - auto goal = std::shared_ptr(request, &request->goal); - GoalResponse user_response = handle_goal_(uuid, goal); + uint64_t intra_process_action_client_id = data->first; + GoalRequestSharedPtr goal_request = data->second; - auto ros_response = std::make_shared(); - ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || - GoalResponse::ACCEPT_AND_DEFER == user_response; - return std::make_pair(user_response, ros_response); - } + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + GoalUUID uuid = get_goal_id_from_goal_request(goal_request.get()); + convert(uuid, &goal_info); - /// \internal - CancelResponse - call_handle_cancel_callback(const GoalUUID & uuid) override - { - std::shared_ptr> goal_handle; + // Call user's callback, getting the user's response and a ros message to send back + auto response_pair = call_handle_goal_callback(uuid, goal_request); + + using Response = typename ActionT::Impl::SendGoalService::Response; + auto goal_response = std::static_pointer_cast(response_pair.second); + + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra_process_action_send_goal_response called " + "after destruction of intra process manager"); + } + + // Here we store the uuid of the goal and associate it with a client + // so we can retrieve it when response is ready, or when sending feedback + // since the feedback calls only provide the goal UUID + // Store an entry + ipm->store_intra_process_action_client_goal_uuid( + intra_process_action_client_id, + std::hash()(uuid)); + + ipm->intra_process_action_send_goal_response( + intra_process_action_client_id, + std::move(goal_response)); + + const auto user_response = response_pair.first; + + // if goal is accepted, create a goal handle, and store it + if (GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response) { - std::lock_guard lock(goal_handles_mutex_); - auto element = goal_handles_.find(uuid); - if (element != goal_handles_.end()) { - goal_handle = element->second.lock(); + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), "Accepted goal %s", to_string(uuid).c_str()); + + // Hack: Get rcl goal handle for simplicity + auto handle = this->get_rcl_action_goal_handle(goal_info, uuid); + + if (user_response == GoalResponse::ACCEPT_AND_EXECUTE) { + // Change status to executing + rcl_ret_t ret = rcl_action_update_goal_state(handle.get(), GOAL_EVENT_EXECUTE); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } } + + // publish status since a goal's state has changed (was accepted or has begun execution) + // This part would be the IPC version of publish_status(); + auto status_msg = this->get_status_array(); + + ipm->template intra_process_action_publish_status( + intra_process_action_client_id, + std::move(status_msg)); + + call_goal_accepted_callback(handle, uuid, std::static_pointer_cast(goal_request)); + } + } + + + // Intra-process version of execute_cancel_request_received_ + // Missing: Deep comparison of functionality betwen IPC on/off + void + ipc_execute_cancel_request_received(CancelRequestDataPairSharedPtr data) + { + uint64_t intra_process_action_client_id = data->first; + CancelRequestSharedPtr request = data->second; + + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra_process_action_send_cancel_response called " + " after destruction of intra process manager"); } - CancelResponse resp = CancelResponse::REJECT; - if (goal_handle) { - resp = handle_cancel_(goal_handle); - if (CancelResponse::ACCEPT == resp) { - try { - goal_handle->_cancel_goal(); - } catch (const rclcpp::exceptions::RCLError & ex) { - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp_action"), - "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); - return CancelResponse::REJECT; - } + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); + cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; + cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + + auto response = process_cancel_request(cancel_request); + + if (!response->goals_canceling.empty()) { + // at least one goal state changed, publish a new status message + auto status_msg = this->get_status_array(); + + ipm->template intra_process_action_publish_status( + intra_process_action_client_id, + std::move(status_msg)); + } + + ipm->intra_process_action_send_cancel_response( + intra_process_action_client_id, + std::move(response)); + } + + + // Intra-process version of execute_result_request_received_ + // See if we can call the server.cpp version of it without doing rcl_action_send_result_response + void + ipc_execute_result_request_received(ResultRequestDataPairSharedPtr data) + { + uint64_t intra_process_action_client_id = data->first; + ResultRequestSharedPtr result_request = data->second; + + // check if the goal exists. How? + GoalUUID uuid = get_goal_id_from_result_request(result_request.get()); + rcl_action_goal_info_t goal_info; + convert(uuid, &goal_info); + + // This is a workaround, I have to find a place to have the + // result response stored somewhere. + std::shared_ptr result_response = this->get_result_response(uuid); + + // Check if a result is already available. If not, it will + // be sent when ready in the on_terminal_state callback below. + if (result_response) { + // Send the result now + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra_process_action_send_result_response called " + "after destruction of intra process manager"); } + + auto typed_response = std::static_pointer_cast(result_response); + ipm->intra_process_action_send_result_response( + intra_process_action_client_id, + std::move(typed_response)); } - return resp; } + // ----------------------------------------------------- + // API for communication between ServerBase and Server<> + /// \internal void call_goal_accepted_callback( @@ -460,44 +650,129 @@ class Server : public ServerBase, public std::enable_shared_from_this> goal_handle; std::weak_ptr> weak_this = this->shared_from_this(); - std::function)> on_terminal_state = + // Define callbacks for the ServerGoalHandle, which will be called from the user APP when + // for example goal_handle->succeed(result) or goal_handle->publish_feedback(feedback); + auto on_terminal_state = [weak_this](const GoalUUID & goal_uuid, std::shared_ptr result_message) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } - // Send result message to anyone that asked - shared_this->publish_result(goal_uuid, result_message); - // Publish a status message any time a goal handle changes state - shared_this->publish_status(); + + std::lock_guard ipc_lock(shared_this->ipc_mutex_); + + if (shared_this->use_intra_process_) { + auto ipm = shared_this->weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process send called after " + "destruction of intra process manager"); + } + + size_t hashed_uuid = std::hash()(goal_uuid); + + // This part would be the IPC version of publish_result(); + // It does not perform any checks, like if the goal exists + uint64_t ipc_action_client_id = + ipm->get_action_client_id_from_goal_uuid(hashed_uuid); + + auto typed_response = std::static_pointer_cast(result_message); + ipm->template intra_process_action_send_result_response( + ipc_action_client_id, + std::move(typed_response)); + + // This part would be the IPC version of publish_status(); + auto status_msg = shared_this->get_status_array(); + + ipm->template intra_process_action_publish_status( + ipc_action_client_id, + std::move(status_msg)); + + ipm->remove_intra_process_action_client_goal_uuid(hashed_uuid); + } else { + // Send result message to anyone that asked + shared_this->publish_result(goal_uuid, result_message); + // Publish a status message any time a goal handle changes state + shared_this->publish_status(); + } + // notify base so it can recalculate the expired goal timer shared_this->notify_goal_terminal_state(); + // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires) std::lock_guard lock(shared_this->goal_handles_mutex_); shared_this->goal_handles_.erase(goal_uuid); }; - std::function on_executing = + auto on_executing = [weak_this](const GoalUUID & goal_uuid) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } - (void)goal_uuid; + + std::lock_guard lock(shared_this->ipc_mutex_); + // Publish a status message any time a goal handle changes state - shared_this->publish_status(); + if (shared_this->use_intra_process_) { + auto ipm = shared_this->weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra_process_action_publish_status called " + "after destruction of intra process manager"); + } + + size_t hashed_uuid = std::hash()(goal_uuid); + + uint64_t ipc_action_client_id = + ipm->get_action_client_id_from_goal_uuid(hashed_uuid); + + // This part would be the IPC version of publish_status(); + auto status_msg = shared_this->get_status_array(); + + ipm->template intra_process_action_publish_status( + ipc_action_client_id, + std::move(status_msg)); + + } else { + shared_this->publish_status(); + } }; - std::function)> publish_feedback = - [weak_this](std::shared_ptr feedback_msg) + + using FeedbackMsg = typename ActionT::Impl::FeedbackMessage; + + auto publish_feedback = + [weak_this](typename std::shared_ptr feedback_msg) { std::shared_ptr> shared_this = weak_this.lock(); if (!shared_this) { return; } - shared_this->publish_feedback(std::static_pointer_cast(feedback_msg)); + + std::lock_guard lock(shared_this->ipc_mutex_); + + if (shared_this->use_intra_process_) { + auto ipm = shared_this->weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "intra_process_action_publish_feedback called " + "after destruction of intra process manager"); + } + + size_t hashed_uuid = std::hash()(feedback_msg->goal_id.uuid); + + uint64_t ipc_action_client_id = + ipm->get_action_client_id_from_goal_uuid(hashed_uuid); + + ipm->template intra_process_action_publish_feedback( + ipc_action_client_id, + std::move(feedback_msg)); + } else { + shared_this->publish_feedback(std::static_pointer_cast(feedback_msg)); + } }; auto request = std::static_pointer_cast< @@ -513,6 +788,51 @@ class Server : public ServerBase, public std::enable_shared_from_this> + call_handle_goal_callback(GoalUUID & uuid, std::shared_ptr message) override + { + auto request = std::static_pointer_cast< + typename ActionT::Impl::SendGoalService::Request>(message); + auto goal = std::shared_ptr(request, &request->goal); + GoalResponse user_response = handle_goal_(uuid, goal); + + auto ros_response = std::make_shared(); + ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response || + GoalResponse::ACCEPT_AND_DEFER == user_response; + return std::make_pair(user_response, ros_response); + } + + /// \internal + CancelResponse + call_handle_cancel_callback(const GoalUUID & uuid) override + { + std::shared_ptr> goal_handle; + { + std::lock_guard lock(goal_handles_mutex_); + auto element = goal_handles_.find(uuid); + if (element != goal_handles_.end()) { + goal_handle = element->second.lock(); + } + } + + CancelResponse resp = CancelResponse::REJECT; + if (goal_handle) { + resp = handle_cancel_(goal_handle); + if (CancelResponse::ACCEPT == resp) { + try { + goal_handle->_cancel_goal(); + } catch (const rclcpp::exceptions::RCLError & ex) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what()); + return CancelResponse::REJECT; + } + } + } + return resp; + } + /// \internal GoalUUID get_goal_id_from_goal_request(void * message) override @@ -565,6 +885,68 @@ class Server : public ServerBase, public std::enable_shared_from_this goal_handles_; std::mutex goal_handles_mutex_; + + using ActionServerIntraProcessT = rclcpp::experimental::ActionServerIntraProcess; + std::shared_ptr ipc_action_server_; + std::recursive_mutex ipc_mutex_; + bool use_intra_process_{false}; + + void + create_intra_process_action_server( + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + const std::string & name, + const rcl_action_server_options_t & options) + { + // Setup intra process if requested. + auto keep_last = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + if (options.goal_service_qos.history != keep_last || + options.result_service_qos.history != keep_last || + options.cancel_service_qos.history != keep_last) + { + throw std::invalid_argument( + "intraprocess communication allowed only with keep last history qos policy"); + } + + if (options.goal_service_qos.depth == 0 || + options.result_service_qos.depth == 0 || + options.cancel_service_qos.depth == 0) + { + throw std::invalid_argument( + "intraprocess communication is not allowed with 0 depth qos policy"); + } + + auto durability_vol = RMW_QOS_POLICY_DURABILITY_VOLATILE; + if (options.goal_service_qos.durability != durability_vol || + options.result_service_qos.durability != durability_vol || + options.cancel_service_qos.durability != durability_vol) + { + throw std::invalid_argument( + "intraprocess communication allowed only with volatile durability"); + } + + rcl_action_server_depth_t qos_history; + qos_history.goal_service_depth = options.goal_service_qos.history; + qos_history.result_service_depth = options.result_service_qos.history; + qos_history.cancel_service_depth = options.cancel_service_qos.history; + + std::lock_guard lock(ipc_mutex_); + + use_intra_process_ = true; + // Create a ActionClientIntraProcess which will be given + // to the intra-process manager. + auto context = node_base->get_context(); + ipc_action_server_ = std::make_shared( + context, name, qos_history, + std::bind(&Server::ipc_execute_goal_request_received, this, std::placeholders::_1), + std::bind(&Server::ipc_execute_cancel_request_received, this, std::placeholders::_1), + std::bind(&Server::ipc_execute_result_request_received, this, std::placeholders::_1)); + + // Add it to the intra process manager. + using rclcpp::experimental::IntraProcessManager; + auto ipm = context->get_sub_context(); + uint64_t ipc_action_client_id = ipm->add_intra_process_action_server(ipc_action_server_); + this->setup_intra_process(ipc_action_client_id, ipm); + } }; } // namespace rclcpp_action #endif // RCLCPP_ACTION__SERVER_HPP_ diff --git a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp index ac9dd49492..81e964cc4e 100644 --- a/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp +++ b/rclcpp_action/include/rclcpp_action/server_goal_handle.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rcl_action/types.h" #include "rcl_action/goal_handle.h" diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2d5018d5af..64178663fa 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -686,4 +686,35 @@ ClientBase::execute(std::shared_ptr & data) } } +void +ClientBase::setup_intra_process( + uint64_t ipc_action_client_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + weak_ipm_ = weak_ipm; + use_intra_process_ = true; + ipc_action_client_id_ = ipc_action_client_id; +} + +rclcpp::Waitable::SharedPtr +ClientBase::get_intra_process_waitable() +{ + std::lock_guard lock(ipc_mutex_); + + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "rclcpp_action::ClientBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the intra-process client from the intra-process manager. + return ipm->get_action_client_intra_process(ipc_action_client_id_); +} + } // namespace rclcpp_action diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index c0fa96a88e..9e4e11399f 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -352,36 +352,8 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) // if goal is accepted, create a goal handle, and store it if (GoalResponse::ACCEPT_AND_EXECUTE == status || GoalResponse::ACCEPT_AND_DEFER == status) { RCLCPP_DEBUG(pimpl_->logger_, "Accepted goal %s", to_string(uuid).c_str()); - // rcl_action will set time stamp - auto deleter = [](rcl_action_goal_handle_t * ptr) - { - if (nullptr != ptr) { - rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); - if (RCL_RET_OK != fail_ret) { - RCLCPP_DEBUG( - rclcpp::get_logger("rclcpp_action"), - "failed to fini rcl_action_goal_handle_t in deleter"); - } - delete ptr; - } - }; - rcl_action_goal_handle_t * rcl_handle; - { - std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); - rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); - } - if (!rcl_handle) { - throw std::runtime_error("Failed to accept new goal\n"); - } - - std::shared_ptr handle(new rcl_action_goal_handle_t, deleter); - // Copy out goal handle since action server storage disappears when it is fini'd - *handle = *rcl_handle; - { - std::lock_guard lock(pimpl_->unordered_map_mutex_); - pimpl_->goal_handles_[uuid] = handle; - } + auto handle = get_rcl_action_goal_handle(goal_info, uuid); if (GoalResponse::ACCEPT_AND_EXECUTE == status) { // Change status to executing @@ -399,30 +371,51 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) data.reset(); } -void -ServerBase::execute_cancel_request_received(std::shared_ptr & data) + +std::shared_ptr +ServerBase::get_rcl_action_goal_handle( + rcl_action_goal_info_t goal_info, + GoalUUID uuid) { - auto shared_ptr = std::static_pointer_cast - , - rmw_request_id_t>>(data); - auto ret = std::get<0>(*shared_ptr); - if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { - // Ignore take failure because connext fails if it receives a sample without valid data. - // This happens when a client shuts down and connext receives a sample saying the client is - // no longer alive. - return; - } else if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret); + // This api is a workaround for IPC communication. + // The goal would be not use any rcl stuff. + auto deleter = [](rcl_action_goal_handle_t * ptr) + { + if (nullptr != ptr) { + rcl_ret_t fail_ret = rcl_action_goal_handle_fini(ptr); + if (RCL_RET_OK != fail_ret) { + RCLCPP_DEBUG( + rclcpp::get_logger("rclcpp_action"), + "failed to fini rcl_action_goal_handle_t in deleter"); + } + delete ptr; + } + }; + rcl_action_goal_handle_t * rcl_handle; + { + std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); + rcl_handle = rcl_action_accept_new_goal(pimpl_->action_server_.get(), &goal_info); + } + if (!rcl_handle) { + throw std::runtime_error("Failed to accept new goal\n"); } - auto request = std::get<1>(*shared_ptr); - auto request_header = std::get<2>(*shared_ptr); - pimpl_->cancel_request_ready_ = false; - // Convert c++ message to C message - rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); - convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); - cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; - cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + std::shared_ptr handle(new rcl_action_goal_handle_t, deleter); + // Copy out goal handle since action server storage disappears when it is fini'd + *handle = *rcl_handle; + + { + std::lock_guard lock(pimpl_->unordered_map_mutex_); + pimpl_->goal_handles_[uuid] = handle; + } + + return handle; +} + +std::shared_ptr +ServerBase::process_cancel_request(rcl_action_cancel_request_t & cancel_request) +{ + rcl_ret_t ret; // Get a list of goal info that should be attempted to be cancelled rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); @@ -472,6 +465,35 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) response->return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED; } + return response; +} + +void +ServerBase::execute_cancel_request_received(std::shared_ptr & data) +{ + auto shared_ptr = std::static_pointer_cast + , + rmw_request_id_t>>(data); + auto ret = std::get<0>(*shared_ptr); + if (RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { + // Ignore take failure because connext fails if it receives a sample without valid data. + // This happens when a client shuts down and connext receives a sample saying the client is + // no longer alive. + return; + } else if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + auto request = std::get<1>(*shared_ptr); + auto request_header = std::get<2>(*shared_ptr); + + // Convert c++ message to C message + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + convert(request->goal_info.goal_id.uuid, &cancel_request.goal_info); + cancel_request.goal_info.stamp.sec = request->goal_info.stamp.sec; + cancel_request.goal_info.stamp.nanosec = request->goal_info.stamp.nanosec; + + auto response = process_cancel_request(cancel_request); + if (!response->goals_canceling.empty()) { // at least one goal state changed, publish a new status message publish_status(); @@ -545,6 +567,18 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) data.reset(); } +// Todo: Use an intra-process way to store goal_results, when using IPC +std::shared_ptr +ServerBase::get_result_response(GoalUUID uuid) +{ + std::lock_guard lock(pimpl_->unordered_map_mutex_); + auto iter = pimpl_->goal_results_.find(uuid); + if (iter != pimpl_->goal_results_.end()) { + return iter->second; + } + return nullptr; +} + void ServerBase::execute_check_expired_goals() { @@ -574,11 +608,9 @@ ServerBase::execute_check_expired_goals() } } -void -ServerBase::publish_status() +std::shared_ptr +ServerBase::get_status_array() { - rcl_ret_t ret; - // We need to hold the lock across this entire method because // rcl_action_server_get_goal_handles() returns an internal pointer to the // goal data. @@ -587,7 +619,7 @@ ServerBase::publish_status() // Get all goal handles known to C action server rcl_action_goal_handle_t ** goal_handles = NULL; size_t num_goals = 0; - ret = rcl_action_server_get_goal_handles( + rcl_ret_t ret = rcl_action_server_get_goal_handles( pimpl_->action_server_.get(), &goal_handles, &num_goals); if (RCL_RET_OK != ret) { @@ -625,8 +657,16 @@ ServerBase::publish_status() status_msg->status_list.push_back(msg); } + return status_msg; +} + +void +ServerBase::publish_status() +{ + auto status_msg = get_status_array(); + // Publish the message through the status publisher - ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); + rcl_ret_t ret = rcl_action_publish_status(pimpl_->action_server_.get(), status_msg.get()); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); @@ -833,3 +873,34 @@ ServerBase::clear_on_ready_callback() entity_type_to_on_ready_callback_.clear(); } + +void +ServerBase::setup_intra_process( + uint64_t ipc_action_server_id, + IntraProcessManagerWeakPtr weak_ipm) +{ + weak_ipm_ = weak_ipm; + use_intra_process_ = true; + ipc_action_server_id_ = ipc_action_server_id; +} + +rclcpp::Waitable::SharedPtr +ServerBase::get_intra_process_waitable() +{ + std::lock_guard lock(ipc_mutex_); + + // If not using intra process, shortcut to nullptr. + if (!use_intra_process_) { + return nullptr; + } + // Get the intra process manager. + auto ipm = weak_ipm_.lock(); + if (!ipm) { + throw std::runtime_error( + "rclcpp_action::ServerBase::get_intra_process_waitable() called " + "after destruction of intra process manager"); + } + + // Use the id to retrieve the intra-process client from the intra-process manager. + return ipm->get_action_server_intra_process(ipc_action_server_id_); +} diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..6145bf6805 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -81,6 +81,13 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf state_machine_options.enable_com_interface = enable_communication_interface; state_machine_options.allocator = node_options->allocator; + rclcpp::IntraProcessSetting ipc_setting; + if (node_base_interface_->get_use_intra_process_default()) { + ipc_setting = rclcpp::IntraProcessSetting::Enable; + } else { + ipc_setting = rclcpp::IntraProcessSetting::Disable; + } + // The call to initialize the state machine takes // currently five different typesupports for all publishers/services // created within the RCL_LIFECYCLE structure. @@ -116,9 +123,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf any_cb.set(std::move(cb)); srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), + node_base_interface_, &state_machine_.com_interface.srv_change_state, - any_cb); + any_cb, + ipc_setting); + node_services_interface_->add_service( std::dynamic_pointer_cast(srv_change_state_), nullptr); @@ -132,9 +141,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf any_cb.set(std::move(cb)); srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), + node_base_interface_, &state_machine_.com_interface.srv_get_state, - any_cb); + any_cb, + ipc_setting); + node_services_interface_->add_service( std::dynamic_pointer_cast(srv_get_state_), nullptr); @@ -148,9 +159,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf any_cb.set(std::move(cb)); srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), + node_base_interface_, &state_machine_.com_interface.srv_get_available_states, - any_cb); + any_cb, + ipc_setting); + node_services_interface_->add_service( std::dynamic_pointer_cast(srv_get_available_states_), nullptr); @@ -165,9 +178,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf srv_get_available_transitions_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), + node_base_interface_, &state_machine_.com_interface.srv_get_available_transitions, - any_cb); + any_cb, + ipc_setting); + node_services_interface_->add_service( std::dynamic_pointer_cast(srv_get_available_transitions_), nullptr); @@ -182,9 +197,11 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf srv_get_transition_graph_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), + node_base_interface_, &state_machine_.com_interface.srv_get_transition_graph, - any_cb); + any_cb, + ipc_setting); + node_services_interface_->add_service( std::dynamic_pointer_cast(srv_get_transition_graph_), nullptr); diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d09f44831c..bda352d3c2 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -30,6 +30,7 @@ #include "rcl_lifecycle/rcl_lifecycle.h" +#include "rclcpp/intra_process_setting.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp"