diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index b6fec7d..daa5973 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -4,31 +4,95 @@ project(rclcpp) find_package(ament_cmake REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(rmw REQUIRED) +find_package(rosidl_generator_cpp REQUIRED) + +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") +endif() + +include_directories(include) + +set(${PROJECT_NAME}_SRCS + src/rclcpp/any_executable.cpp + src/rclcpp/callback_group.cpp + src/rclcpp/client.cpp + src/rclcpp/context.cpp + src/rclcpp/contexts/default_context.cpp + src/rclcpp/executor.cpp + src/rclcpp/executors.cpp + src/rclcpp/executors/multi_threaded_executor.cpp + src/rclcpp/executors/single_threaded_executor.cpp + src/rclcpp/intra_process_manager.cpp + src/rclcpp/intra_process_manager_state.cpp + src/rclcpp/memory_strategies.cpp + src/rclcpp/memory_strategy.cpp + src/rclcpp/parameter.cpp + src/rclcpp/parameter_client.cpp + src/rclcpp/parameter_service.cpp + src/rclcpp/publisher.cpp + src/rclcpp/node.cpp + src/rclcpp/service.cpp + src/rclcpp/subscription.cpp + src/rclcpp/timer.cpp + src/rclcpp/utilities.cpp +) +if(WIN32) + list(APPEND ${PROJECT_NAME}_SRCS src/rclcpp/windows_helper.cpp) +endif() +add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SRCS}) +ament_target_dependencies( + ${PROJECT_NAME} + "rcl_interfaces" + "rmw" + "rosidl_generator_cpp" +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +# It also avoids using certain parts of the code that the consuming code must +# use, but which the library cannot consume itself. +target_compile_definitions(${PROJECT_NAME} PRIVATE "RCLCPP_BUILDING_LIBRARY") +if(APPLE) + # Since the rmw_* symbols are unresolved at the time of building librclcpp, + # tell the linker on OS X to dynamically look them up at runtime. + set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") +endif() -ament_export_dependencies(rmw) ament_export_dependencies(rcl_interfaces) +ament_export_dependencies(rmw) +ament_export_dependencies(rosidl_generator_cpp) ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + if(AMENT_ENABLE_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - find_package(rmw REQUIRED) - - if(NOT WIN32) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra") - endif() - - include_directories(include) - ament_add_gtest(test_function_traits test/test_function_traits.cpp) + if(TARGET test_function_traits) + target_include_directories(test_function_traits PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ) + endif() ament_add_gtest(test_mapped_ring_buffer test/test_mapped_ring_buffer.cpp) + if(TARGET test_mapped_ring_buffer) + target_include_directories(test_mapped_ring_buffer PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ) + endif() ament_add_gtest(test_intra_process_manager test/test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) target_include_directories(test_intra_process_manager PUBLIC - "${rcl_interfaces_INCLUDE_DIRS}" - "${rmw_INCLUDE_DIRS}") + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ) endif() endif() @@ -45,3 +109,10 @@ install( DIRECTORY src/ DESTINATION src/rclcpp ) + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 1bdddb8..46950f4 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -28,9 +29,10 @@ namespace executor struct AnyExecutable { RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable); - AnyExecutable() - : subscription(0), timer(0), callback_group(0), node(0) - {} + + RCLCPP_PUBLIC + AnyExecutable(); + // Only one of the following pointers will be set. rclcpp::subscription::SubscriptionBase::SharedPtr subscription; rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process; diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 8618810..e77040a 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -22,6 +22,8 @@ #include #include +#include "rclcpp/function_traits.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 94e26a1..555d1d7 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -23,6 +23,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp index ca050fa..deb395e 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -23,6 +23,7 @@ #include #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -49,77 +50,57 @@ class CallbackGroup public: RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup); - CallbackGroup(CallbackGroupType group_type) - : type_(group_type), can_be_taken_from_(true) - {} + RCLCPP_PUBLIC + explicit CallbackGroup(CallbackGroupType group_type); - const std::vector & - get_subscription_ptrs() const - { - return subscription_ptrs_; - } + RCLCPP_PUBLIC + const std::vector & + get_subscription_ptrs() const; - const std::vector & - get_timer_ptrs() const - { - return timer_ptrs_; - } + RCLCPP_PUBLIC + const std::vector & + get_timer_ptrs() const; - const std::vector & - get_service_ptrs() const - { - return service_ptrs_; - } + RCLCPP_PUBLIC + const std::vector & + get_service_ptrs() const; - const std::vector & - get_client_ptrs() const - { - return client_ptrs_; - } + RCLCPP_PUBLIC + const std::vector & + get_client_ptrs() const; - std::atomic_bool & can_be_taken_from() - { - return can_be_taken_from_; - } + RCLCPP_PUBLIC + std::atomic_bool & + can_be_taken_from(); - const CallbackGroupType & type() const - { - return type_; - } + RCLCPP_PUBLIC + const CallbackGroupType & + type() const; private: RCLCPP_DISABLE_COPY(CallbackGroup); + RCLCPP_PUBLIC void - add_subscription( - const subscription::SubscriptionBase::SharedPtr subscription_ptr) - { - subscription_ptrs_.push_back(subscription_ptr); - } + add_subscription(const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr); + RCLCPP_PUBLIC void - add_timer(const timer::TimerBase::SharedPtr timer_ptr) - { - timer_ptrs_.push_back(timer_ptr); - } + add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr); + RCLCPP_PUBLIC void - add_service(const service::ServiceBase::SharedPtr service_ptr) - { - service_ptrs_.push_back(service_ptr); - } + add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr); + RCLCPP_PUBLIC void - add_client(const client::ClientBase::SharedPtr client_ptr) - { - client_ptrs_.push_back(client_ptr); - } + add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr); CallbackGroupType type_; - std::vector subscription_ptrs_; - std::vector timer_ptrs_; - std::vector service_ptrs_; - std::vector client_ptrs_; + std::vector subscription_ptrs_; + std::vector timer_ptrs_; + std::vector service_ptrs_; + std::vector client_ptrs_; std::atomic_bool can_be_taken_from_; }; diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 7037363..dab0025 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -27,6 +27,7 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -40,32 +41,22 @@ class ClientBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase); + RCLCPP_PUBLIC ClientBase( std::shared_ptr node_handle, rmw_client_t * client_handle, - const std::string & service_name) - : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) - {} + const std::string & service_name); - virtual ~ClientBase() - { - if (client_handle_) { - if (rmw_destroy_client(client_handle_) != RMW_RET_OK) { - fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); - } - } - } + RCLCPP_PUBLIC + virtual ~ClientBase(); - const std::string & get_service_name() const - { - return this->service_name_; - } + RCLCPP_PUBLIC + const std::string & + get_service_name() const; - const rmw_client_t * get_client_handle() const - { - return this->client_handle_; - } + RCLCPP_PUBLIC + const rmw_client_t * + get_client_handle() const; virtual std::shared_ptr create_response() = 0; virtual std::shared_ptr create_request_header() = 0; diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 1c56cbb..b800772 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -26,6 +26,8 @@ #include #include +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -38,7 +40,7 @@ class Context public: RCLCPP_SMART_PTR_DEFINITIONS(Context); - Context() {} + Context(); template std::shared_ptr diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index c10225e..2c70714 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -16,6 +16,7 @@ #define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -29,16 +30,13 @@ class DefaultContext : public rclcpp::context::Context public: RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext); - DefaultContext() {} - + RCLCPP_PUBLIC + DefaultContext(); }; +RCLCPP_PUBLIC DefaultContext::SharedPtr -get_global_default_context() -{ - static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); - return default_context; -} +get_global_default_context(); } // namespace default_context } // namespace contexts diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index cc3fe42..49b4749 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -23,14 +23,13 @@ #include #include -#include "rcl_interfaces/msg/intra_process_message.hpp" - #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/memory_strategies.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -54,29 +53,18 @@ public: /// Default constructor. // \param[in] ms The memory strategy to be used with this executor. - explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : interrupt_guard_condition_(rmw_create_guard_condition()), - memory_strategy_(ms) - { - } + RCLCPP_PUBLIC + explicit Executor( + memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy()); /// Default destructor. - virtual ~Executor() - { - // Try to deallocate the interrupt guard condition. - if (interrupt_guard_condition_ != nullptr) { - rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); - } - } - } + RCLCPP_PUBLIC + virtual ~Executor(); /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely. // It is up to the implementation of Executor to implement spin. - virtual void spin() = 0; + virtual void + spin() = 0; /// Add a node to the executor. /** @@ -86,26 +74,9 @@ public: * the executor is blocked at the rmw layer while waiting for work and it is notified that a new * node was added, it will wake up. */ + RCLCPP_PUBLIC virtual void - add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true) - { - // Check to ensure node not already added - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node == node_ptr) { - // TODO(jacquelinekay): Use a different error here? - throw std::runtime_error("Cannot add node to executor, node already added."); - } - } - weak_nodes_.push_back(node_ptr); - if (notify) { - // Interrupt waiting to handle new node - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - } - } + add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true); /// Remove a node from the executor. /** @@ -114,29 +85,9 @@ public: * This is useful if the last node was removed from the executor while the executor was blocked * waiting for work in another thread, because otherwise the executor would never be notified. */ + RCLCPP_PUBLIC virtual void - remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true) - { - bool node_removed = false; - weak_nodes_.erase( - std::remove_if( - weak_nodes_.begin(), weak_nodes_.end(), - [&](std::weak_ptr & i) - { - bool matched = (i.lock() == node_ptr); - node_removed |= matched; - return matched; - })); - if (notify) { - // If the node was matched and removed, interrupt waiting - if (node_removed) { - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - } - } - } + remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true); /// Add a node to executor, execute the next available unit of work, and remove the node. /** @@ -146,28 +97,23 @@ public: * function to be non-blocking. */ template - void spin_node_once(rclcpp::node::Node::SharedPtr node, + void + spin_node_once(rclcpp::node::Node::SharedPtr node, std::chrono::duration timeout = std::chrono::duration(-1)) { - this->add_node(node, false); - // non-blocking = true - auto any_exec = get_next_executable(timeout); - if (any_exec) { - execute_any_executable(any_exec); - } - this->remove_node(node, false); + return spin_node_once_nanoseconds( + node, + std::chrono::duration_cast(timeout) + ); } /// Add a node, complete all immediately available work, and remove the node. /** * \param[in] node Shared pointer to the node to add. */ - void spin_node_some(rclcpp::node::Node::SharedPtr node) - { - this->add_node(node, false); - spin_some(); - this->remove_node(node, false); - } + RCLCPP_PUBLIC + void + spin_node_some(rclcpp::node::Node::SharedPtr node); /// Complete all available queued work without blocking. /** @@ -176,14 +122,9 @@ public: * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function * to block (which may have unintended consequences). */ - virtual void spin_some() - { - while (AnyExecutable::SharedPtr any_exec = - get_next_executable(std::chrono::milliseconds::zero())) - { - execute_any_executable(any_exec); - } - } + RCLCPP_PUBLIC + virtual void + spin_some(); /// Support dynamic switching of the memory strategy. /** @@ -191,411 +132,72 @@ public: * unintended consequences. * \param[in] memory_strategy Shared pointer to the memory strategy to set. */ + RCLCPP_PUBLIC void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy) - { - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); - } - memory_strategy_ = memory_strategy; - } + set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy); protected: + RCLCPP_PUBLIC + void + spin_node_once_nanoseconds(rclcpp::node::Node::SharedPtr node, std::chrono::nanoseconds timeout); + /// Find the next available executable and do the work associated with it. /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, * service, client). */ + RCLCPP_PUBLIC void - execute_any_executable(AnyExecutable::SharedPtr any_exec) - { - if (!any_exec) { - return; - } - if (any_exec->timer) { - execute_timer(any_exec->timer); - } - if (any_exec->subscription) { - execute_subscription(any_exec->subscription); - } - if (any_exec->subscription_intra_process) { - execute_intra_process_subscription(any_exec->subscription_intra_process); - } - if (any_exec->service) { - execute_service(any_exec->service); - } - if (any_exec->client) { - execute_client(any_exec->client); - } - // Reset the callback_group, regardless of type - any_exec->callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - } + execute_any_executable(AnyExecutable::SharedPtr any_exec); + RCLCPP_PUBLIC static void execute_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr subscription) - { - std::shared_ptr message = subscription->create_message(); - bool taken = false; - rmw_message_info_t message_info; - auto ret = - rmw_take_with_info(subscription->get_subscription_handle(), - message.get(), &taken, &message_info); - if (ret == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = false; - subscription->handle_message(message, message_info); - } - } else { - fprintf(stderr, - "[rclcpp::error] take failed for subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); - } - subscription->return_message(message); - } + rclcpp::subscription::SubscriptionBase::SharedPtr subscription); + RCLCPP_PUBLIC static void execute_intra_process_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr subscription) - { - rcl_interfaces::msg::IntraProcessMessage ipm; - bool taken = false; - rmw_message_info_t message_info; - rmw_ret_t status = rmw_take_with_info( - subscription->get_intra_process_subscription_handle(), - &ipm, - &taken, - &message_info); - if (status == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } - } else { - fprintf(stderr, - "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); - } - } + rclcpp::subscription::SubscriptionBase::SharedPtr subscription); + RCLCPP_PUBLIC static void - execute_timer( - rclcpp::timer::TimerBase::SharedPtr timer) - { - timer->execute_callback(); - } + execute_timer(rclcpp::timer::TimerBase::SharedPtr timer); + RCLCPP_PUBLIC static void - execute_service( - rclcpp::service::ServiceBase::SharedPtr service) - { - std::shared_ptr request_header = service->create_request_header(); - std::shared_ptr request = service->create_request(); - bool taken = false; - rmw_ret_t status = rmw_take_request( - service->get_service_handle(), - request_header.get(), - request.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { - service->handle_request(request_header, request); - } - } else { - fprintf(stderr, - "[rclcpp::error] take request failed for server of service '%s': %s\n", - service->get_service_name().c_str(), rmw_get_error_string_safe()); - } - } + execute_service(rclcpp::service::ServiceBase::SharedPtr service); + RCLCPP_PUBLIC static void - execute_client( - rclcpp::client::ClientBase::SharedPtr client) - { - std::shared_ptr request_header = client->create_request_header(); - std::shared_ptr response = client->create_response(); - bool taken = false; - rmw_ret_t status = rmw_take_response( - client->get_client_handle(), - request_header.get(), - response.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { - client->handle_response(request_header, response); - } - } else { - fprintf(stderr, - "[rclcpp::error] take response failed for client of service '%s': %s\n", - client->get_service_name().c_str(), rmw_get_error_string_safe()); - } - } + execute_client(rclcpp::client::ClientBase::SharedPtr client); -/*** Populate class storage from stored weak node pointers and wait. ***/ - - template + RCLCPP_PUBLIC void - wait_for_work(std::chrono::duration timeout = std::chrono::duration(-1)) - { - memory_strategy_->clear_active_entities(); + wait_for_work(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // Collect the subscriptions and timers to be waited on - bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); - - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_nodes) { - weak_nodes_.erase( - remove_if(weak_nodes_.begin(), weak_nodes_.end(), - [](std::weak_ptr i) - { - return i.expired(); - })); - } - - // Use the number of subscriptions to allocate memory in the handles - rmw_subscriptions_t subscriber_handles; - subscriber_handles.subscriber_count = - memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers); - - rmw_services_t service_handles; - service_handles.service_count = - memory_strategy_->fill_service_handles(service_handles.services); - - rmw_clients_t client_handles; - client_handles.client_count = - memory_strategy_->fill_client_handles(client_handles.clients); - - // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) - size_t number_of_guard_conds = 2; - rmw_guard_conditions_t guard_condition_handles; - guard_condition_handles.guard_condition_count = number_of_guard_conds; - guard_condition_handles.guard_conditions = static_cast(guard_cond_handles_.data()); - if (guard_condition_handles.guard_conditions == NULL && - number_of_guard_conds > 0) - { - // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? - throw std::runtime_error("Could not malloc for guard condition pointers."); - } - // Put the global ctrl-c guard condition in - assert(guard_condition_handles.guard_condition_count > 1); - guard_condition_handles.guard_conditions[0] = \ - rclcpp::utilities::get_global_sigint_guard_condition()->data; - // Put the executor's guard condition in - guard_condition_handles.guard_conditions[1] = \ - interrupt_guard_condition_->data; - - rmw_time_t * wait_timeout = NULL; - rmw_time_t rmw_timeout; - - auto next_timer_duration = get_earliest_timer(); - // If the next timer timeout must preempt the requested timeout - // or if the requested timeout blocks forever, and there exists a valid timer, - // replace the requested timeout with the next timeout. - bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero(); - if ((next_timer_duration < timeout || - timeout < std::chrono::duration::zero()) && has_valid_timer) - { - rmw_timeout.sec = - std::chrono::duration_cast(next_timer_duration).count(); - rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } else if (timeout >= std::chrono::duration::zero()) { - // Convert timeout representation to rmw_time - rmw_timeout.sec = std::chrono::duration_cast(timeout).count(); - rmw_timeout.nsec = std::chrono::duration_cast(timeout).count() % - (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } - - // Now wait on the waitable subscriptions and timers - rmw_ret_t status = rmw_wait( - &subscriber_handles, - &guard_condition_handles, - &service_handles, - &client_handles, - wait_timeout); - if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - // If ctrl-c guard condition, return directly - if (guard_condition_handles.guard_conditions[0] != 0) { - // Make sure to free or clean memory - memory_strategy_->clear_handles(); - return; - } - - memory_strategy_->revalidate_handles(); - } - -/******************************/ + RCLCPP_PUBLIC rclcpp::node::Node::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) - { - if (!group) { - return rclcpp::node::Node::SharedPtr(); - } - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; - } - } - } - return rclcpp::node::Node::SharedPtr(); - } + get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group); + RCLCPP_PUBLIC rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_timer( - rclcpp::timer::TimerBase::SharedPtr timer) - { - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_timer : group->get_timer_ptrs()) { - auto t = weak_timer.lock(); - if (t == timer) { - return group; - } - } - } - } - return rclcpp::callback_group::CallbackGroup::SharedPtr(); - } + get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer); + RCLCPP_PUBLIC void - get_next_timer(AnyExecutable::SharedPtr any_exec) - { - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - auto timer = timer_ref.lock(); - if (timer && timer->check_and_trigger()) { - any_exec->timer = timer; - any_exec->callback_group = group; - node = get_node_by_group(group); - return; - } - } - } - } - } + get_next_timer(AnyExecutable::SharedPtr any_exec); + RCLCPP_PUBLIC std::chrono::nanoseconds - get_earliest_timer() - { - std::chrono::nanoseconds latest = std::chrono::nanoseconds::max(); - bool timers_empty = true; - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - timers_empty = false; - // Check the expected trigger time - auto timer = timer_ref.lock(); - if (timer && timer->time_until_trigger() < latest) { - latest = timer->time_until_trigger(); - } - } - } - } - if (timers_empty) { - return std::chrono::nanoseconds(-1); - } - return latest; - } + get_earliest_timer(); + RCLCPP_PUBLIC AnyExecutable::SharedPtr - get_next_ready_executable() - { - auto any_exec = memory_strategy_->instantiate_next_executable(); - // Check the timers to see if there are any that are ready, if so return - get_next_timer(any_exec); - if (any_exec->timer) { - return any_exec; - } - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_exec, weak_nodes_); - if (any_exec->subscription || any_exec->subscription_intra_process) { - return any_exec; - } - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_exec, weak_nodes_); - if (any_exec->service) { - return any_exec; - } - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_exec, weak_nodes_); - if (any_exec->client) { - return any_exec; - } - // If there is no ready executable, return a null ptr - return nullptr; - } + get_next_ready_executable(); - template + RCLCPP_PUBLIC AnyExecutable::SharedPtr - get_next_executable(std::chrono::duration timeout = std::chrono::duration(-1)) - { - // Check to see if there are any subscriptions or timers needing service - // TODO(wjwwood): improve run to run efficiency of this function - auto any_exec = get_next_ready_executable(); - // If there are none - if (!any_exec) { - // Wait for subscriptions or timers to work on - wait_for_work(timeout); - // Try again - any_exec = get_next_ready_executable(); - } - // At this point any_exec should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (any_exec) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly - if (any_exec->callback_group && any_exec->callback_group->type() == \ - callback_group::CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise - assert(any_exec->callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - any_exec->callback_group->can_be_taken_from().store(false); - } - } - return any_exec; - } + get_next_executable(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /// Guard condition for signaling the rmw layer to wake up for special events. rmw_guard_condition_t * interrupt_guard_condition_; diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp index 045cc48..64aaa20 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -20,9 +20,23 @@ #include #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { + +/// Create a default single-threaded executor and execute any immediately available work. +// \param[in] node_ptr Shared pointer to the node to spin. +RCLCPP_PUBLIC +void +spin_some(node::Node::SharedPtr node_ptr); + +/// Create a default single-threaded executor and spin the specified node. +// \param[in] node_ptr Shared pointer to the node to spin. +RCLCPP_PUBLIC +void +spin(node::Node::SharedPtr node_ptr); + namespace executors { @@ -37,8 +51,7 @@ using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; */ enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; -/// Spin (blocking) until the future is complete, until the function times out (if applicable), -/// or until rclcpp is interrupted. +/// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted. /** * \param[in] executor The executor which will spin the node. * \param[in] node_ptr The node to spin. @@ -55,7 +68,7 @@ spin_node_until_future_complete( std::shared_future & future, std::chrono::duration timeout = std::chrono::duration(-1)) { - // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete + // TODO(wjwwood): does not work recursively; can't call spin_node_until_future_complete // inside a callback executed by an executor. // Check the future before entering the while loop. @@ -81,7 +94,18 @@ spin_node_until_future_complete( return FutureReturnCode::INTERRUPTED; } -} // namespace executors -} // namespace rclcpp +} // namespace executors + +template +rclcpp::executors::FutureReturnCode +spin_until_future_complete( + node::Node::SharedPtr node_ptr, std::shared_future & future, + std::chrono::duration timeout = std::chrono::duration(-1)) +{ + rclcpp::executors::SingleThreadedExecutor executor; + return executors::spin_node_until_future_complete(executor, node_ptr, future, timeout); +} + +} // namespace rclcpp #endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 82b1f84..7b57eb0 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -27,6 +27,8 @@ #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -40,63 +42,32 @@ class MultiThreadedExecutor : public executor::Executor public: RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor); - MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : executor::Executor(ms) - { - number_of_threads_ = std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) { - number_of_threads_ = 1; - } - } + RCLCPP_PUBLIC + MultiThreadedExecutor( + memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy()); - virtual ~MultiThreadedExecutor() {} + RCLCPP_PUBLIC + virtual ~MultiThreadedExecutor(); + RCLCPP_PUBLIC void - spin() - { - std::vector threads; - { - std::lock_guard wait_lock(wait_mutex_); - size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id` - for (size_t i = number_of_threads_; i > 0; --i) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++); - threads.emplace_back(func); - } - } - for (auto & thread : threads) { - thread.join(); - } - } + spin(); + RCLCPP_PUBLIC size_t - get_number_of_threads() - { - return number_of_threads_; - } + get_number_of_threads(); + +protected: + RCLCPP_PUBLIC + void + run(size_t this_thread_number); private: - void run(size_t this_thread_id) - { - rclcpp::thread_id = this_thread_id; - while (rclcpp::utilities::ok()) { - executor::AnyExecutable::SharedPtr any_exec; - { - std::lock_guard wait_lock(wait_mutex_); - if (!rclcpp::utilities::ok()) { - return; - } - any_exec = get_next_executable(); - } - execute_any_executable(any_exec); - } - } - RCLCPP_DISABLE_COPY(MultiThreadedExecutor); std::mutex wait_mutex_; size_t number_of_threads_; + std::unordered_map thread_number_by_thread_id_; }; } // namespace multi_threaded_executor diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index be87e19..3302a68 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -28,6 +28,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/rate.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -44,23 +45,20 @@ public: RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor); /// Default constructor. See the default constructor for Executor. - SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : executor::Executor(ms) {} + RCLCPP_PUBLIC + SingleThreadedExecutor( + memory_strategy::MemoryStrategy::SharedPtr ms = memory_strategies::create_default_strategy()); /// Default destrcutor. - virtual ~SingleThreadedExecutor() {} + RCLCPP_PUBLIC + virtual ~SingleThreadedExecutor(); /// Single-threaded implementation of spin. // This function will block until work comes in, execute it, and keep blocking. // It will only be interrupt by a CTRL-C (managed by the global signal handler). - void spin() - { - while (rclcpp::utilities::ok()) { - auto any_exec = get_next_executable(); - execute_any_executable(any_exec); - } - } + RCLCPP_PUBLIC + void + spin(); private: RCLCPP_DISABLE_COPY(SingleThreadedExecutor); diff --git a/rclcpp/include/rclcpp/intra_process_manager.hpp b/rclcpp/include/rclcpp/intra_process_manager.hpp index d49d91c..96c682d 100644 --- a/rclcpp/include/rclcpp/intra_process_manager.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager.hpp @@ -32,6 +32,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/subscription.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -124,12 +125,12 @@ private: public: RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager); + RCLCPP_PUBLIC explicit IntraProcessManager( - IntraProcessManagerStateBase::SharedPtr state = create_default_state() - ) - : state_(state) - { - } + IntraProcessManagerStateBase::SharedPtr state = create_default_state()); + + RCLCPP_PUBLIC + virtual ~IntraProcessManager(); /// Register a subscription with the manager, returns subscriptions unique id. /* In addition to generating a unique intra process id for the subscription, @@ -143,24 +144,18 @@ public: * \param subscription the Subscription to register. * \return an unsigned 64-bit integer which is the subscription's unique id. */ + RCLCPP_PUBLIC uint64_t - add_subscription(subscription::SubscriptionBase::SharedPtr subscription) - { - auto id = IntraProcessManager::get_next_unique_id(); - state_->add_subscription(id, subscription); - return id; - } + add_subscription(subscription::SubscriptionBase::SharedPtr subscription); /// Unregister a subscription using the subscription's unique id. /* This method does not allocate memory. * * \param intra_process_subscription_id id of the subscription to remove. */ + RCLCPP_PUBLIC void - remove_subscription(uint64_t intra_process_subscription_id) - { - state_->remove_subscription(intra_process_subscription_id); - } + remove_subscription(uint64_t intra_process_subscription_id); /// Register a publisher with the manager, returns the publisher unique id. /* In addition to generating and returning a unique id for the publisher, @@ -203,11 +198,9 @@ public: * * \param intra_process_publisher_id id of the publisher to remove. */ + RCLCPP_PUBLIC void - remove_publisher(uint64_t intra_process_publisher_id) - { - state_->remove_publisher(intra_process_publisher_id); - } + remove_publisher(uint64_t intra_process_publisher_id); /// Store a message in the manager, and return the message sequence number. /* The given message is stored in internal storage using the given publisher @@ -336,41 +329,18 @@ public: } /// Return true if the given rmw_gid_t matches any stored Publishers. + RCLCPP_PUBLIC bool - matches_any_publishers(const rmw_gid_t * id) const - { - return state_->matches_any_publishers(id); - } + matches_any_publishers(const rmw_gid_t * id) const; private: - static uint64_t get_next_unique_id() - { - auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed); - // Check for rollover (we started at 1). - if (0 == next_id) { - // This puts a technical limit on the number of times you can add a publisher or subscriber. - // But even if you could add (and remove) them at 1 kHz (very optimistic rate) - // it would still be a very long time before you could exhaust the pool of id's: - // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years - // So around 585 million years. Even at 1 GHz, it would take 585 years. - // I think it's safe to avoid trying to handle overflow. - // If we roll over then it's most likely a bug. - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::overflow_error( - "exhausted the unique id's for publishers and subscribers in this process " - "(congratulations your computer is either extremely fast or extremely old)"); - // *INDENT-ON* - } - return next_id; - } - - static std::atomic next_unique_id_; + RCLCPP_PUBLIC + static uint64_t + get_next_unique_id(); IntraProcessManagerStateBase::SharedPtr state_; }; -std::atomic IntraProcessManager::next_unique_id_ {1}; - } // namespace intra_process_manager } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/intra_process_manager_state.hpp b/rclcpp/include/rclcpp/intra_process_manager_state.hpp index 8a4bc5e..f35c8d8 100644 --- a/rclcpp/include/rclcpp/intra_process_manager_state.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager_state.hpp @@ -30,6 +30,8 @@ #include #include +#include "rclcpp/visibility_control.hpp" + namespace rclcpp { namespace intra_process_manager @@ -40,6 +42,9 @@ class IntraProcessManagerStateBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase); + IntraProcessManagerStateBase() = default; + ~IntraProcessManagerStateBase() = default; + virtual void add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0; @@ -70,12 +75,18 @@ public: virtual bool matches_any_publishers(const rmw_gid_t * id) const = 0; + +private: + RCLCPP_DISABLE_COPY(IntraProcessManagerStateBase); }; template> class IntraProcessManagerState : public IntraProcessManagerStateBase { public: + IntraProcessManagerState() = default; + ~IntraProcessManagerState() = default; + void add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) { @@ -222,6 +233,8 @@ public: } private: + RCLCPP_DISABLE_COPY(IntraProcessManagerState); + template using RebindAlloc = typename std::allocator_traits::template rebind_alloc; @@ -259,10 +272,9 @@ private: PublisherMap publishers_; }; -static IntraProcessManagerStateBase::SharedPtr create_default_state() -{ - return std::make_shared>(); -} +RCLCPP_PUBLIC +IntraProcessManagerStateBase::SharedPtr +create_default_state(); } // namespace intra_process_manager } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp index 8a9b7f9..ce66ceb 100644 --- a/rclcpp/include/rclcpp/mapped_ring_buffer.hpp +++ b/rclcpp/include/rclcpp/mapped_ring_buffer.hpp @@ -23,13 +23,14 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { namespace mapped_ring_buffer { -class MappedRingBufferBase +class RCLCPP_PUBLIC MappedRingBufferBase { public: RCLCPP_SMART_PTR_DEFINITIONS(MappedRingBufferBase); diff --git a/rclcpp/include/rclcpp/memory_strategies.hpp b/rclcpp/include/rclcpp/memory_strategies.hpp index f5a1a2a..d938840 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -17,18 +17,16 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { namespace memory_strategies { -using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; - -static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy() -{ - return std::make_shared>(); -} +RCLCPP_PUBLIC +memory_strategy::MemoryStrategy::SharedPtr +create_default_strategy(); } // namespace memory_strategies } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/memory_strategy.hpp b/rclcpp/include/rclcpp/memory_strategy.hpp index 895ea9d..a7ec910 100644 --- a/rclcpp/include/rclcpp/memory_strategy.hpp +++ b/rclcpp/include/rclcpp/memory_strategy.hpp @@ -21,6 +21,7 @@ #include "rclcpp/any_executable.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -33,11 +34,11 @@ namespace memory_strategy * the rmw implementation after the executor waits for work, based on the number of entities that * come through. */ -class MemoryStrategy +class RCLCPP_PUBLIC MemoryStrategy { public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy); - using WeakNodeVector = std::vector>; + using WeakNodeVector = std::vector>; // return the new number of subscribers virtual size_t fill_subscriber_handles(void ** & ptr) = 0; @@ -56,200 +57,50 @@ public: /// Provide a newly initialized AnyExecutable object. // \return Shared pointer to the fresh executable. - virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; + virtual rclcpp::executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; virtual void - get_next_subscription(executor::AnyExecutable::SharedPtr any_exec, + get_next_subscription(rclcpp::executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) = 0; virtual void - get_next_service(executor::AnyExecutable::SharedPtr any_exec, + get_next_service(rclcpp::executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) = 0; virtual void - get_next_client(executor::AnyExecutable::SharedPtr any_exec, + get_next_client(rclcpp::executor::AnyExecutable::SharedPtr any_exec, const WeakNodeVector & weak_nodes) = 0; static rclcpp::subscription::SubscriptionBase::SharedPtr get_subscription_by_handle(void * subscriber_handle, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_subscription : group->get_subscription_ptrs()) { - auto subscription = weak_subscription.lock(); - if (subscription) { - if (subscription->get_subscription_handle()->data == subscriber_handle) { - return subscription; - } - if (subscription->get_intra_process_subscription_handle() && - subscription->get_intra_process_subscription_handle()->data == subscriber_handle) - { - return subscription; - } - } - } - } - } - return nullptr; - } + const WeakNodeVector & weak_nodes); static rclcpp::service::ServiceBase::SharedPtr - get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & service : group->get_service_ptrs()) { - if (service->get_service_handle()->data == service_handle) { - return service; - } - } - } - } - return nullptr; - } + get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes); static rclcpp::client::ClientBase::SharedPtr - get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & client : group->get_client_ptrs()) { - if (client->get_client_handle()->data == client_handle) { - return client; - } - } - } - } - return nullptr; - } + get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes); static rclcpp::node::Node::SharedPtr get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeVector & weak_nodes) - { - if (!group) { - return nullptr; - } - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; - } - } - } - return nullptr; - } + const WeakNodeVector & weak_nodes); static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_subscription( rclcpp::subscription::SubscriptionBase::SharedPtr subscription, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_sub : group->get_subscription_ptrs()) { - auto sub = weak_sub.lock(); - if (sub == subscription) { - return group; - } - } - } - } - return nullptr; - } + const WeakNodeVector & weak_nodes); static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_service( rclcpp::service::ServiceBase::SharedPtr service, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & serv : group->get_service_ptrs()) { - if (serv == service) { - return group; - } - } - } - } - return nullptr; - } + const WeakNodeVector & weak_nodes); static rclcpp::callback_group::CallbackGroup::SharedPtr get_group_by_client(rclcpp::client::ClientBase::SharedPtr client, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & cli : group->get_client_ptrs()) { - if (cli == client) { - return group; - } - } - } - } - return nullptr; - } - - + const WeakNodeVector & weak_nodes); }; } // namespace memory_strategy - } // namespace rclcpp #endif // RCLCPP__MEMORY_STRATEGY_HPP_ diff --git a/rclcpp/include/rclcpp/message_memory_strategy.hpp b/rclcpp/include/rclcpp/message_memory_strategy.hpp index 61b582d..00b2a9a 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -19,6 +19,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 0e78b10..b95c6ba 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -26,7 +26,6 @@ #include "rcl_interfaces/msg/parameter_descriptor.hpp" #include "rcl_interfaces/msg/parameter_event.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" -#include "rosidl_generator_cpp/message_type_support.hpp" #include "rclcpp/callback_group.hpp" #include "rclcpp/client.hpp" @@ -38,7 +37,7 @@ #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/timer.hpp" - +#include "rclcpp/visibility_control.hpp" // Forward declaration of ROS middleware class namespace rmw @@ -63,6 +62,7 @@ public: * \param[in] use_intra_process_comms True to use the optimized intra-process communication * pipeline to pass messages between nodes in the same process using shared memory. */ + RCLCPP_PUBLIC explicit Node(const std::string & node_name, bool use_intra_process_comms = false); /// Create a node based on the node name and a rclcpp::context::Context. @@ -72,16 +72,19 @@ public: * \param[in] use_intra_process_comms True to use the optimized intra-process communication * pipeline to pass messages between nodes in the same process using shared memory. */ + RCLCPP_PUBLIC Node( const std::string & node_name, rclcpp::context::Context::SharedPtr context, bool use_intra_process_comms = false); /// Get the name of the node. // \return The name of the node. + RCLCPP_PUBLIC const std::string & - get_name() const {return name_; } + get_name() const; /// Create and return a callback group. + RCLCPP_PUBLIC rclcpp::callback_group::CallbackGroup::SharedPtr create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); @@ -168,13 +171,14 @@ public: * \param[in] callback User-defined callback function. * \param[in] group Callback group to execute this timer's callback in. */ + RCLCPP_PUBLIC rclcpp::timer::WallTimer::SharedPtr create_wall_timer( std::chrono::nanoseconds period, rclcpp::timer::CallbackType callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - /// Create a timer with a sub-nanosecond precision update period. + /// Create a timer. /** * \param[in] period Time interval between triggers of the callback. * \param[in] callback User-defined callback function. @@ -206,37 +210,50 @@ public: FunctorT callback, rclcpp::callback_group::CallbackGroup::SharedPtr group = nullptr); - std::vector set_parameters( - const std::vector & parameters); + RCLCPP_PUBLIC + std::vector + set_parameters(const std::vector & parameters); - rcl_interfaces::msg::SetParametersResult set_parameters_atomically( - const std::vector & parameters); + RCLCPP_PUBLIC + rcl_interfaces::msg::SetParametersResult + set_parameters_atomically(const std::vector & parameters); - std::vector get_parameters( - const std::vector & names) const; + RCLCPP_PUBLIC + std::vector + get_parameters(const std::vector & names) const; - std::vector describe_parameters( - const std::vector & names) const; + RCLCPP_PUBLIC + std::vector + describe_parameters(const std::vector & names) const; - std::vector get_parameter_types( - const std::vector & names) const; + RCLCPP_PUBLIC + std::vector + get_parameter_types(const std::vector & names) const; - rcl_interfaces::msg::ListParametersResult list_parameters( - const std::vector & prefixes, uint64_t depth) const; + RCLCPP_PUBLIC + rcl_interfaces::msg::ListParametersResult + list_parameters(const std::vector & prefixes, uint64_t depth) const; - std::map get_topic_names_and_types() const; + RCLCPP_PUBLIC + std::map + get_topic_names_and_types() const; - size_t count_publishers(const std::string & topic_name) const; + RCLCPP_PUBLIC + size_t + count_publishers(const std::string & topic_name) const; - size_t count_subscribers(const std::string & topic_name) const; + RCLCPP_PUBLIC + size_t + count_subscribers(const std::string & topic_name) const; - const CallbackGroupWeakPtrList & get_callback_groups() const; + RCLCPP_PUBLIC + const CallbackGroupWeakPtrList & + get_callback_groups() const; private: RCLCPP_DISABLE_COPY(Node); - static const rosidl_message_type_support_t * ipm_ts_; - + RCLCPP_PUBLIC bool group_in_node(callback_group::CallbackGroup::SharedPtr group); @@ -263,20 +280,8 @@ private: publisher::Publisher::SharedPtr events_publisher_; }; -const rosidl_message_type_support_t * Node::ipm_ts_ = - rosidl_generator_cpp::get_message_type_support_handle(); - -} /* namespace node */ -} /* namespace rclcpp */ - -#define RCLCPP_REGISTER_NODE(Class) RMW_EXPORT \ - rclcpp::node::Node::SharedPtr \ - create_node() \ - { \ - return rclcpp::node::Node::SharedPtr(new Class( \ - rclcpp::contexts::default_context::DefaultContext:: \ - make_shared())); \ - } +} // namespace node +} // namespace rclcpp #ifndef RCLCPP__NODE_IMPL_HPP_ // Template implementations diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp new file mode 100644 index 0000000..d4d3042 --- /dev/null +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -0,0 +1,359 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__NODE_IMPL_HPP_ +#define RCLCPP__NODE_IMPL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/intra_process_manager.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +#ifndef RCLCPP__NODE_HPP_ +#include "node.hpp" +#endif + +namespace rclcpp +{ +namespace node +{ + +template +typename rclcpp::publisher::Publisher::SharedPtr +Node::create_publisher( + const std::string & topic_name, size_t qos_history_depth, + std::shared_ptr allocator) +{ + if (!allocator) { + allocator = std::make_shared(); + } + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = qos_history_depth; + return this->create_publisher(topic_name, qos, allocator); +} + +template +typename publisher::Publisher::SharedPtr +Node::create_publisher( + const std::string & topic_name, const rmw_qos_profile_t & qos_profile, + std::shared_ptr allocator) +{ + if (!allocator) { + allocator = std::make_shared(); + } + using rosidl_generator_cpp::get_message_type_support_handle; + auto type_support_handle = get_message_type_support_handle(); + rmw_publisher_t * publisher_handle = rmw_create_publisher( + node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile); + if (!publisher_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create publisher: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + auto publisher = publisher::Publisher::make_shared( + node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator); + + if (use_intra_process_comms_) { + rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher( + node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_name + "__intra").c_str(), qos_profile); + if (!intra_process_publisher_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create intra process publisher: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + auto intra_process_manager = + context_->get_sub_context(); + uint64_t intra_process_publisher_id = + intra_process_manager->add_publisher(publisher); + rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; + // *INDENT-OFF* + auto shared_publish_callback = + [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t + { + auto ipm = weak_ipm.lock(); + if (!ipm) { + // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? + throw std::runtime_error( + "intra process publish called after destruction of intra process manager"); + } + if (!msg) { + throw std::runtime_error("cannot publisher msg which is a null pointer"); + } + auto & message_type_info = typeid(MessageT); + if (message_type_info != type_info) { + throw std::runtime_error( + std::string("published type '") + type_info.name() + + "' is incompatible from the publisher type '" + message_type_info.name() + "'"); + } + MessageT * typed_message_ptr = static_cast(msg); + using MessageDeleter = typename publisher::Publisher::MessageDeleter; + std::unique_ptr unique_msg(typed_message_ptr); + uint64_t message_seq = + ipm->store_intra_process_message(publisher_id, unique_msg); + return message_seq; + }; + // *INDENT-ON* + publisher->setup_intra_process( + intra_process_publisher_id, + shared_publish_callback, + intra_process_publisher_handle); + } + return publisher; +} + +template +typename rclcpp::subscription::Subscription::SharedPtr +Node::create_subscription( + const std::string & topic_name, + CallbackT callback, + const rmw_qos_profile_t & qos_profile, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat, + typename std::shared_ptr allocator) +{ + if (!allocator) { + allocator = std::make_shared(); + } + + rclcpp::subscription::AnySubscriptionCallback any_subscription_callback(allocator); + any_subscription_callback.set(callback); + + using rosidl_generator_cpp::get_message_type_support_handle; + + if (!msg_mem_strat) { + msg_mem_strat = + rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); + } + + auto type_support_handle = get_message_type_support_handle(); + rmw_subscription_t * subscriber_handle = rmw_create_subscription( + node_handle_.get(), type_support_handle, + topic_name.c_str(), qos_profile, ignore_local_publications); + if (!subscriber_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create subscription: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + using rclcpp::subscription::Subscription; + using rclcpp::subscription::SubscriptionBase; + + auto sub = Subscription::make_shared( + node_handle_, + subscriber_handle, + topic_name, + ignore_local_publications, + any_subscription_callback, + msg_mem_strat); + auto sub_base_ptr = std::dynamic_pointer_cast(sub); + // Setup intra process. + if (use_intra_process_comms_) { + rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription( + node_handle_.get(), rclcpp::type_support::get_intra_process_message_msg_type_support(), + (topic_name + "__intra").c_str(), qos_profile, false); + if (!subscriber_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create intra process subscription: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + auto intra_process_manager = + context_->get_sub_context(); + rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; + uint64_t intra_process_subscription_id = + intra_process_manager->add_subscription(sub_base_ptr); + // *INDENT-OFF* + sub->setup_intra_process( + intra_process_subscription_id, + intra_process_subscriber_handle, + [weak_ipm]( + uint64_t publisher_id, + uint64_t message_sequence, + uint64_t subscription_id, + typename Subscription::MessageUniquePtr & message) + { + auto ipm = weak_ipm.lock(); + if (!ipm) { + // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? + throw std::runtime_error( + "intra process take called after destruction of intra process manager"); + } + ipm->take_intra_process_message( + publisher_id, message_sequence, subscription_id, message); + }, + [weak_ipm](const rmw_gid_t * sender_gid) -> bool { + auto ipm = weak_ipm.lock(); + if (!ipm) { + throw std::runtime_error( + "intra process publisher check called after destruction of intra process manager"); + } + return ipm->matches_any_publishers(sender_gid); + } + ); + // *INDENT-ON* + } + // Assign to a group. + if (group) { + if (!group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create subscription, group not in node."); + } + group->add_subscription(sub_base_ptr); + } else { + default_callback_group_->add_subscription(sub_base_ptr); + } + number_of_subscriptions_++; + return sub; +} + +template +typename rclcpp::subscription::Subscription::SharedPtr +Node::create_subscription( + const std::string & topic_name, + size_t qos_history_depth, + CallbackT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group, + bool ignore_local_publications, + typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr + msg_mem_strat, + std::shared_ptr allocator) +{ + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.depth = qos_history_depth; + return this->create_subscription( + topic_name, + callback, + qos, + group, + ignore_local_publications, + msg_mem_strat, + allocator); +} + +template +typename client::Client::SharedPtr +Node::create_client( + const std::string & service_name, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + using rosidl_generator_cpp::get_service_type_support_handle; + auto service_type_support_handle = + get_service_type_support_handle(); + + rmw_client_t * client_handle = rmw_create_client( + this->node_handle_.get(), service_type_support_handle, service_name.c_str()); + if (!client_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create client: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + using rclcpp::client::Client; + using rclcpp::client::ClientBase; + + auto cli = Client::make_shared( + node_handle_, + client_handle, + service_name); + + auto cli_base_ptr = std::dynamic_pointer_cast(cli); + if (group) { + if (!group_in_node(group)) { + // TODO(esteve): use custom exception + throw std::runtime_error("Cannot create client, group not in node."); + } + group->add_client(cli_base_ptr); + } else { + default_callback_group_->add_client(cli_base_ptr); + } + number_of_clients_++; + + return cli; +} + +template +typename rclcpp::service::Service::SharedPtr +Node::create_service( + const std::string & service_name, + FunctorT callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + using rosidl_generator_cpp::get_service_type_support_handle; + auto service_type_support_handle = + get_service_type_support_handle(); + + rclcpp::service::AnyServiceCallback any_service_callback; + any_service_callback.set(callback); + + rmw_service_t * service_handle = rmw_create_service( + node_handle_.get(), service_type_support_handle, service_name.c_str()); + if (!service_handle) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("could not create service: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + auto serv = service::Service::make_shared( + node_handle_, service_handle, service_name, any_service_callback); + auto serv_base_ptr = std::dynamic_pointer_cast(serv); + if (group) { + if (!group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create service, group not in node."); + } + group->add_service(serv_base_ptr); + } else { + default_callback_group_->add_service(serv_base_ptr); + } + number_of_services_++; + return serv; +} + +} // namespace node +} // namespace rclcpp + +#endif // RCLCPP__NODE_IMPL_HPP_ diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index f10be34..6c036e5 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -24,6 +24,8 @@ #include #include #include +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" namespace rclcpp { @@ -45,87 +47,40 @@ enum ParameterType class ParameterVariant { public: - ParameterVariant() - : name_("") - { - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; - } - explicit ParameterVariant(const std::string & name, const bool bool_value) - : name_(name) - { - value_.bool_value = bool_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - } - explicit ParameterVariant(const std::string & name, const int int_value) - : name_(name) - { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - } - explicit ParameterVariant(const std::string & name, const int64_t int_value) - : name_(name) - { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - } - explicit ParameterVariant(const std::string & name, const float double_value) - : name_(name) - { - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - } - explicit ParameterVariant(const std::string & name, const double double_value) - : name_(name) - { - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - } - explicit ParameterVariant(const std::string & name, const std::string & string_value) - : name_(name) - { - value_.string_value = string_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - } - explicit ParameterVariant(const std::string & name, const char * string_value) - : ParameterVariant(name, std::string(string_value)) {} - explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) - : name_(name) - { - value_.bytes_value = bytes_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES; - } + RCLCPP_PUBLIC + ParameterVariant(); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const bool bool_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const int int_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const int64_t int_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const float double_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const double double_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const std::string & string_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const char * string_value); + RCLCPP_PUBLIC + explicit ParameterVariant(const std::string & name, const std::vector & bytes_value); - inline ParameterType get_type() const {return static_cast(value_.type); } + RCLCPP_PUBLIC + ParameterType + get_type() const; - inline std::string get_type_name() const - { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return "bool"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return "integer"; - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return "double"; - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return "string"; - case rclcpp::parameter::ParameterType::PARAMETER_BYTES: - return "bytes"; - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } - } + RCLCPP_PUBLIC + std::string + get_type_name() const; - inline std::string get_name() const & {return name_; } + RCLCPP_PUBLIC + const std::string & + get_name() const; - inline rcl_interfaces::msg::ParameterValue get_parameter_value() const - { - return value_; - } + RCLCPP_PUBLIC + rcl_interfaces::msg::ParameterValue + get_parameter_value() const; template typename std::enable_if::type @@ -179,86 +134,37 @@ public: return value_.bytes_value; } - int64_t as_int() const {return get_value(); } + RCLCPP_PUBLIC + int64_t + as_int() const; - double as_double() const {return get_value(); } + RCLCPP_PUBLIC + double + as_double() const; - const std::string & as_string() const {return get_value(); } + RCLCPP_PUBLIC + const std::string & + as_string() const; - bool as_bool() const {return get_value(); } + RCLCPP_PUBLIC + bool + as_bool() const; - const std::vector & as_bytes() const - { - return get_value(); - } + RCLCPP_PUBLIC + const std::vector & + as_bytes() const; - static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter) - { - switch (parameter.value.type) { - case PARAMETER_BOOL: - return ParameterVariant(parameter.name, parameter.value.bool_value); - case PARAMETER_INTEGER: - return ParameterVariant(parameter.name, parameter.value.integer_value); - case PARAMETER_DOUBLE: - return ParameterVariant(parameter.name, parameter.value.double_value); - case PARAMETER_STRING: - return ParameterVariant(parameter.name, parameter.value.string_value); - case PARAMETER_BYTES: - return ParameterVariant(parameter.name, parameter.value.bytes_value); - case PARAMETER_NOT_SET: - throw std::runtime_error("Type from ParameterValue is not set"); - default: - // TODO(wjwwood): use custom exception - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type)); - // *INDENT-ON* - } - } + RCLCPP_PUBLIC + static ParameterVariant + from_parameter(const rcl_interfaces::msg::Parameter & parameter); - rcl_interfaces::msg::Parameter to_parameter() - { - rcl_interfaces::msg::Parameter parameter; - parameter.name = name_; - parameter.value = value_; - return parameter; - } + RCLCPP_PUBLIC + rcl_interfaces::msg::Parameter + to_parameter(); - std::string value_to_string() const - { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return as_bool() ? "true" : "false"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return std::to_string(as_int()); - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return std::to_string(as_double()); - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return as_string(); - case rclcpp::parameter::ParameterType::PARAMETER_BYTES: - { - std::stringstream bytes; - bool first_byte = true; - bytes << "[" << std::hex; - for (auto & byte : as_bytes()) { - bytes << "0x" << byte; - if (!first_byte) { - bytes << ", "; - } else { - first_byte = false; - } - } - return bytes.str(); - } - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } - } + RCLCPP_PUBLIC + std::string + value_to_string() const; private: std::string name_; @@ -267,71 +173,34 @@ private: /* Return a json encoded version of the parameter intended for a dict. */ -std::string _to_json_dict_entry(const ParameterVariant & param) -{ - std::stringstream ss; - ss << "\"" << param.get_name() << "\": "; - ss << "{\"type\": \"" << param.get_type_name() << "\", "; - ss << "\"value\": \"" << param.value_to_string() << "\"}"; - return ss.str(); -} +RCLCPP_PUBLIC +std::string +_to_json_dict_entry(const ParameterVariant & param); +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv); -} /* namespace parameter */ +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const std::vector & parameters); -} /* namespace rclcpp */ +} // namespace parameter +} // namespace rclcpp namespace std { + /* Return a json encoded version of the parameter intended for a list. */ -inline std::string to_string(const rclcpp::parameter::ParameterVariant & param) -{ - std::stringstream ss; - ss << "{\"name\": \"" << param.get_name() << "\", "; - ss << "\"type\": \"" << param.get_type_name() << "\", "; - ss << "\"value\": \"" << param.value_to_string() << "\"}"; - return ss.str(); -} +RCLCPP_PUBLIC +std::string +to_string(const rclcpp::parameter::ParameterVariant & param); /* Return a json encoded version of a vector of parameters, as a string*/ -inline std::string to_string(const std::vector & parameters) -{ - std::stringstream ss; - ss << "{"; - bool first = true; - for (const auto & pv : parameters) { - if (first == false) { - ss << ", "; - } else { - first = false; - } - ss << rclcpp::parameter::_to_json_dict_entry(pv); - } - ss << "}"; - return ss.str(); -} +RCLCPP_PUBLIC +std::string +to_string(const std::vector & parameters); -} /* namespace std */ - -namespace rclcpp -{ -namespace parameter -{ - -std::ostream & operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv) -{ - os << std::to_string(pv); - return os; -} - -std::ostream & operator<<(std::ostream & os, const std::vector & parameters) -{ - os << std::to_string(parameters); - return os; -} - -} /* namespace parameter */ - -} /* namespace rclcpp */ +} // namespace std #endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */ diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 66314bd..d748fee 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -16,216 +16,80 @@ #define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ #include +#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" namespace rclcpp { - namespace parameter_client { class AsyncParametersClient { - public: RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient); - AsyncParametersClient(const rclcpp::node::Node::SharedPtr node, - const std::string & remote_node_name = "") - : node_(node) - { - if (remote_node_name != "") { - remote_node_name_ = remote_node_name; - } else { - remote_node_name_ = node_->get_name(); - } - get_parameters_client_ = node_->create_client( - remote_node_name_ + "__get_parameters"); - get_parameter_types_client_ = node_->create_client( - remote_node_name_ + "__get_parameter_types"); - set_parameters_client_ = node_->create_client( - remote_node_name_ + "__set_parameters"); - list_parameters_client_ = node_->create_client( - remote_node_name_ + "__list_parameters"); - describe_parameters_client_ = node_->create_client( - remote_node_name_ + "__describe_parameters"); - } + RCLCPP_PUBLIC + AsyncParametersClient( + const rclcpp::node::Node::SharedPtr node, + const std::string & remote_node_name = ""); + RCLCPP_PUBLIC std::shared_future> get_parameters( const std::vector & names, - std::function>)> callback = nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->names = names; - - get_parameters_client_->async_send_request( - request, - [request, promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - std::vector parameter_variants; - auto & pvalues = cb_f.get()->values; - - for (auto & pvalue : pvalues) { - auto i = &pvalue - &pvalues[0]; - rcl_interfaces::msg::Parameter parameter; - parameter.name = request->names[i]; - parameter.value = pvalue; - parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter( - parameter)); - } - - promise_result->set_value(parameter_variants); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } + std::function< + void(std::shared_future>) + > callback = nullptr); + RCLCPP_PUBLIC std::shared_future> get_parameter_types( const std::vector & names, - std::function>)> callback = nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->names = names; - - get_parameter_types_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - std::vector types; - auto & pts = cb_f.get()->types; - for (auto & pt : pts) { - pts.push_back(static_cast(pt)); - } - promise_result->set_value(types); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } + std::function< + void(std::shared_future>) + > callback = nullptr); + RCLCPP_PUBLIC std::shared_future> set_parameters( const std::vector & parameters, - std::function>)> callback = - nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - - std::transform(parameters.begin(), parameters.end(), std::back_inserter( - request->parameters), []( - rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); - - set_parameters_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->results); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } + std::function< + void(std::shared_future>) + > callback = nullptr); + RCLCPP_PUBLIC std::shared_future set_parameters_atomically( const std::vector & parameters, - std::function)> callback = - nullptr) - { - auto promise_result = - std::make_shared>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - - std::transform(parameters.begin(), parameters.end(), std::back_inserter( - request->parameters), []( - rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); - - set_parameters_atomically_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->result); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } + std::function< + void(std::shared_future) + > callback = nullptr); + RCLCPP_PUBLIC std::shared_future list_parameters( const std::vector & prefixes, uint64_t depth, - std::function)> callback = - nullptr) - { - auto promise_result = - std::make_shared>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->prefixes = prefixes; - request->depth = depth; - - list_parameters_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->result); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } + std::function< + void(std::shared_future) + > callback = nullptr); template typename rclcpp::subscription::Subscription::SharedPtr @@ -254,90 +118,36 @@ class SyncParametersClient public: RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient); + RCLCPP_PUBLIC SyncParametersClient( - rclcpp::node::Node::SharedPtr node) - : node_(node) - { - executor_ = std::make_shared(); - async_parameters_client_ = std::make_shared(node); - } + rclcpp::node::Node::SharedPtr node); + RCLCPP_PUBLIC SyncParametersClient( rclcpp::executor::Executor::SharedPtr executor, - rclcpp::node::Node::SharedPtr node) - : executor_(executor), node_(node) - { - async_parameters_client_ = std::make_shared(node); - } + rclcpp::node::Node::SharedPtr node); + RCLCPP_PUBLIC std::vector - get_parameters(const std::vector & parameter_names) - { - auto f = async_parameters_client_->get_parameters(parameter_names); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) - { - return f.get(); - } - // Return an empty vector if unsuccessful - return std::vector(); - } + get_parameters(const std::vector & parameter_names); + RCLCPP_PUBLIC std::vector - get_parameter_types(const std::vector & parameter_names) - { - auto f = async_parameters_client_->get_parameter_types(parameter_names); - - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) - { - return f.get(); - } - return std::vector(); - } + get_parameter_types(const std::vector & parameter_names); + RCLCPP_PUBLIC std::vector - set_parameters(const std::vector & parameters) - { - auto f = async_parameters_client_->set_parameters(parameters); - - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) - { - return f.get(); - } - return std::vector(); - } + set_parameters(const std::vector & parameters); + RCLCPP_PUBLIC rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters) - { - auto f = async_parameters_client_->set_parameters_atomically(parameters); - - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) - { - return f.get(); - } - - throw std::runtime_error("Unable to get result of set parameters service call."); - } + set_parameters_atomically(const std::vector & parameters); + RCLCPP_PUBLIC rcl_interfaces::msg::ListParametersResult list_parameters( const std::vector & parameter_prefixes, - uint64_t depth) - { - auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); - - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) - { - return f.get(); - } - - throw std::runtime_error("Unable to get result of list parameters service call."); - } + uint64_t depth); template typename rclcpp::subscription::Subscription::SharedPtr diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index ffc2983..b350fe5 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -17,19 +17,18 @@ #include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rclcpp/executors.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" namespace rclcpp { @@ -43,119 +42,8 @@ class ParameterService public: RCLCPP_SMART_PTR_DEFINITIONS(ParameterService); - ParameterService(const rclcpp::node::Node::SharedPtr node) - : node_(node) - { - std::weak_ptr captured_node = node_; - get_parameters_service_ = node_->create_service( - node_->get_name() + "__get_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto values = node->get_parameters(request->names); - for (auto & pvariant : values) { - response->values.push_back(pvariant.get_parameter_value()); - } - } - ); - - get_parameter_types_service_ = node_->create_service( - node_->get_name() + "__get_parameter_types", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto types = node->get_parameter_types(request->names); - std::transform(types.cbegin(), types.cend(), - std::back_inserter(response->types), [](const uint8_t & type) { - return static_cast(type); - }); - } - ); - - set_parameters_service_ = node_->create_service( - node_->get_name() + "__set_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - std::vector pvariants; - for (auto & p : request->parameters) { - pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p)); - } - auto results = node->set_parameters(pvariants); - response->results = results; - } - ); - - set_parameters_atomically_service_ = - node_->create_service( - node_->get_name() + "__set_parameters_atomically", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - std::vector pvariants; - std::transform(request->parameters.cbegin(), request->parameters.cend(), - std::back_inserter(pvariants), - [](const rcl_interfaces::msg::Parameter & p) { - return rclcpp::parameter::ParameterVariant:: - from_parameter(p); - }); - auto result = node->set_parameters_atomically(pvariants); - response->result = result; - } - ); - - describe_parameters_service_ = node_->create_service( - node_->get_name() + "__describe_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto descriptors = node->describe_parameters(request->names); - response->descriptors = descriptors; - } - ); - - list_parameters_service_ = node_->create_service( - node_->get_name() + "__list_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto result = node->list_parameters(request->prefixes, request->depth); - response->result = result; - } - ); - - - } + RCLCPP_PUBLIC + explicit ParameterService(const rclcpp::node::Node::SharedPtr node); private: const rclcpp::node::Node::SharedPtr node_; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index d0acec5..b3b851a 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -27,8 +27,10 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rmw/impl/cpp/demangle.hpp" +#include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -55,77 +57,40 @@ public: * \param[in] topic The topic that this publisher publishes on. * \param[in] queue_size The maximum number of unpublished messages to queue. */ + RCLCPP_PUBLIC PublisherBase( std::shared_ptr node_handle, rmw_publisher_t * publisher_handle, std::string topic, - size_t queue_size) - : node_handle_(node_handle), publisher_handle_(publisher_handle), - intra_process_publisher_handle_(nullptr), - topic_(topic), queue_size_(queue_size), - intra_process_publisher_id_(0), store_intra_process_message_(nullptr) - { - // Life time of this object is tied to the publisher handle. - if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - } + size_t queue_size); /// Default destructor. - virtual ~PublisherBase() - { - if (intra_process_publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) { - fprintf( - stderr, - "Error in destruction of intra process rmw publisher handle: %s\n", - rmw_get_error_string_safe()); - } - } - if (publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) { - fprintf( - stderr, - "Error in destruction of rmw publisher handle: %s\n", - rmw_get_error_string_safe()); - } - } - } + RCLCPP_PUBLIC + virtual ~PublisherBase(); /// Get the topic that this publisher publishes on. // \return The topic name. + RCLCPP_PUBLIC const std::string & - get_topic_name() const - { - return topic_; - } + get_topic_name() const; /// Get the queue size for this publisher. // \return The queue size. + RCLCPP_PUBLIC size_t - get_queue_size() const - { - return queue_size_; - } + get_queue_size() const; /// Get the global identifier for this publisher (used in rmw and by DDS). // \return The gid. + RCLCPP_PUBLIC const rmw_gid_t & - get_gid() const - { - return rmw_gid_; - } + get_gid() const; /// Get the global identifier for this publisher used by intra-process communication. // \return The intra-process gid. + RCLCPP_PUBLIC const rmw_gid_t & - get_intra_process_gid() const - { - return intra_process_rmw_gid_; - } + get_intra_process_gid() const; /// Compare this publisher to a gid. /** @@ -133,11 +98,9 @@ public: * \param[in] gid Reference to a gid. * \return True if the publisher's gid matches the input. */ + RCLCPP_PUBLIC bool - operator==(const rmw_gid_t & gid) const - { - return *this == &gid; - } + operator==(const rmw_gid_t & gid) const; /// Compare this publisher to a pointer gid. /** @@ -145,47 +108,19 @@ public: * \param[in] gid A pointer to a gid. * \return True if this publisher's gid matches the input. */ + RCLCPP_PUBLIC bool - operator==(const rmw_gid_t * gid) const - { - bool result = false; - auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); - if (ret != RMW_RET_OK) { - throw std::runtime_error( - std::string("failed to compare gids: ") + rmw_get_error_string_safe()); - } - if (!result) { - ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); - if (ret != RMW_RET_OK) { - throw std::runtime_error( - std::string("failed to compare gids: ") + rmw_get_error_string_safe()); - } - } - return result; - } + operator==(const rmw_gid_t * gid) const; typedef std::function StoreMessageCallbackT; protected: + RCLCPP_PUBLIC void setup_intra_process( uint64_t intra_process_publisher_id, StoreMessageCallbackT callback, - rmw_publisher_t * intra_process_publisher_handle) - { - intra_process_publisher_id_ = intra_process_publisher_id; - store_intra_process_message_ = callback; - intra_process_publisher_handle_ = intra_process_publisher_handle; - // Life time of this object is tied to the publisher handle. - auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to create intra process publisher gid: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - } + rmw_publisher_t * intra_process_publisher_handle); std::shared_ptr node_handle_; diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp index b9bbb23..f4b6f70 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -21,6 +21,7 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -47,11 +48,11 @@ class GenericRate : public RateBase public: RCLCPP_SMART_PTR_DEFINITIONS(GenericRate); - GenericRate(double rate) + explicit GenericRate(double rate) : GenericRate( duration_cast(duration(1.0 / rate))) {} - GenericRate(std::chrono::nanoseconds period) + explicit GenericRate(std::chrono::nanoseconds period) : period_(period), last_interval_(Clock::now()) {} diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index c91d31a..ed7b081 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -25,9 +25,8 @@ #include #include #include +#include "rclcpp/visibility_control.hpp" -namespace rclcpp -{ // NOLINTNEXTLINE(runtime/int) const std::chrono::seconds operator"" _s(unsigned long long s) { @@ -39,32 +38,31 @@ const std::chrono::nanoseconds operator"" _s(long double s) std::chrono::duration(s)); } -const std::chrono::nanoseconds // NOLINTNEXTLINE(runtime/int) -operator"" _ms(unsigned long long ms) +const std::chrono::nanoseconds operator"" _ms(unsigned long long ms) { return std::chrono::milliseconds(ms); } -const std::chrono::nanoseconds -operator"" _ms(long double ms) +const std::chrono::nanoseconds operator"" _ms(long double ms) { return std::chrono::duration_cast( std::chrono::duration(ms)); } -const std::chrono::nanoseconds // NOLINTNEXTLINE(runtime/int) -operator"" _ns(unsigned long long ns) +const std::chrono::nanoseconds operator"" _ns(unsigned long long ns) { return std::chrono::nanoseconds(ns); } -const std::chrono::nanoseconds -operator"" _ns(long double ns) +const std::chrono::nanoseconds operator"" _ns(long double ns) { return std::chrono::duration_cast( std::chrono::duration(ns)); } +namespace rclcpp +{ + // Namespace escalations. // For example, this next line escalates type "rclcpp:node::Node" to "rclcpp::Node" using rclcpp::node::Node; @@ -82,34 +80,6 @@ using rclcpp::utilities::shutdown; using rclcpp::utilities::init; using rclcpp::utilities::sleep_for; -/// Create a default single-threaded executor and execute any immediately available work. -// \param[in] node_ptr Shared pointer to the node to spin. -void spin_some(Node::SharedPtr node_ptr) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.spin_node_some(node_ptr); -} - -/// Create a default single-threaded executor and spin the specified node. -// \param[in] node_ptr Shared pointer to the node to spin. -void spin(Node::SharedPtr node_ptr) -{ - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node_ptr); - executor.spin(); -} - -template -rclcpp::executors::FutureReturnCode -spin_until_future_complete( - Node::SharedPtr node_ptr, std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - rclcpp::executors::SingleThreadedExecutor executor; - return rclcpp::executors::spin_node_until_future_complete( - executor, node_ptr, future, timeout); -} - } /* namespace rclcpp */ #endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2bfbb09..169c0d6 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -26,6 +26,7 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -39,34 +40,22 @@ class ServiceBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); + RCLCPP_PUBLIC ServiceBase( std::shared_ptr node_handle, rmw_service_t * service_handle, - const std::string service_name) - : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) - {} + const std::string service_name); - virtual ~ServiceBase() - { - if (service_handle_) { - if (rmw_destroy_service(service_handle_) != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw service_handle_ handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - } - } + RCLCPP_PUBLIC + virtual ~ServiceBase(); - std::string get_service_name() - { - return this->service_name_; - } + RCLCPP_PUBLIC + std::string + get_service_name(); - const rmw_service_t * get_service_handle() - { - return this->service_handle_; - } + RCLCPP_PUBLIC + const rmw_service_t * + get_service_handle(); virtual std::shared_ptr create_request() = 0; virtual std::shared_ptr create_request_header() = 0; diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 7622123..3bc602e 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,6 +21,7 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 8f9438c..801a8f8 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -17,6 +17,7 @@ #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -32,9 +33,13 @@ namespace message_pool_memory_strategy * The size of the message pool should be at least the largest number of concurrent accesses to * the subscription (usually the number of threads). */ -template::value>::type * = -nullptr> +template< + typename MessageT, + size_t Size, + typename std::enable_if< + rosidl_generator_traits::has_fixed_size::value + >::type * = nullptr +> class MessagePoolMemoryStrategy : public message_memory_strategy::MessageMemoryStrategy { diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index fb9c952..8e5b06a 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -29,6 +29,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -54,75 +55,48 @@ public: * \param[in] topic_name Name of the topic to subscribe to. * \param[in] ignore_local_publications True to ignore local publications (unused). */ + RCLCPP_PUBLIC SubscriptionBase( std::shared_ptr node_handle, rmw_subscription_t * subscription_handle, const std::string & topic_name, - bool ignore_local_publications) - : intra_process_subscription_handle_(nullptr), - node_handle_(node_handle), - subscription_handle_(subscription_handle), - topic_name_(topic_name), - ignore_local_publications_(ignore_local_publications) - { - // To avoid unused private member warnings. - (void)ignore_local_publications_; - } + bool ignore_local_publications); /// Default destructor. - virtual ~SubscriptionBase() - { - if (subscription_handle_) { - if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw subscription handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - } - if (intra_process_subscription_handle_) { - auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_); - if (ret != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw intra process subscription handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - } - } + RCLCPP_PUBLIC + virtual ~SubscriptionBase(); /// Get the topic that this subscription is subscribed on. - const std::string & get_topic_name() const - { - return this->topic_name_; - } + RCLCPP_PUBLIC + const std::string & + get_topic_name() const; - const rmw_subscription_t * get_subscription_handle() const - { - return subscription_handle_; - } + RCLCPP_PUBLIC + const rmw_subscription_t * + get_subscription_handle() const; - const rmw_subscription_t * get_intra_process_subscription_handle() const - { - return intra_process_subscription_handle_; - } + RCLCPP_PUBLIC + const rmw_subscription_t * + get_intra_process_subscription_handle() const; /// Borrow a new message. // \return Shared pointer to the fresh message. - virtual std::shared_ptr create_message() = 0; + virtual std::shared_ptr + create_message() = 0; /// Check if we need to handle the message, and execute the callback if we do. /** * \param[in] message Shared pointer to the message to handle. * \param[in] message_info Metadata associated with this message. */ - virtual void handle_message( - std::shared_ptr & message, - const rmw_message_info_t & message_info) = 0; + virtual void + handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) = 0; /// Return the message borrowed in create_message. // \param[in] Shared pointer to the returned message. - virtual void return_message(std::shared_ptr & message) = 0; - virtual void handle_intra_process_message( + virtual void + return_message(std::shared_ptr & message) = 0; + virtual void + handle_intra_process_message( rcl_interfaces::msg::IntraProcessMessage & ipm, const rmw_message_info_t & message_info) = 0; @@ -140,7 +114,7 @@ private: bool ignore_local_publications_; }; -using namespace any_subscription_callback; +using any_subscription_callback::AnySubscriptionCallback; /// Subscription implementation, templated on the type of message this subscription receives. template> @@ -172,8 +146,8 @@ public: const std::string & topic_name, bool ignore_local_publications, AnySubscriptionCallback callback, - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = - message_memory_strategy::MessageMemoryStrategy::SharedPtr + memory_strategy = message_memory_strategy::MessageMemoryStrategy::create_default()) : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), any_callback_(callback), diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 9a69cb8..b6a25b7 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -27,6 +27,7 @@ #include #include #include +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -42,32 +43,23 @@ class TimerBase public: RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase); - TimerBase(std::chrono::nanoseconds period, CallbackType callback) - : period_(period), - callback_(callback), - canceled_(false) - { - } + RCLCPP_PUBLIC + TimerBase(std::chrono::nanoseconds period, CallbackType callback); - virtual ~TimerBase() - { - } + RCLCPP_PUBLIC + virtual ~TimerBase(); + RCLCPP_PUBLIC void - cancel() - { - this->canceled_ = true; - } + cancel(); - void execute_callback() const - { - callback_(); - } + RCLCPP_PUBLIC + void + execute_callback() const; - const CallbackType & get_callback() const - { - return callback_; - } + RCLCPP_PUBLIC + const CallbackType & + get_callback() const; /// Check how long the timer has until its next scheduled callback. // \return A std::chrono::duration representing the relative time until the next callback. diff --git a/rclcpp/include/rclcpp/type_support_decl.hpp b/rclcpp/include/rclcpp/type_support_decl.hpp new file mode 100644 index 0000000..e417ff8 --- /dev/null +++ b/rclcpp/include/rclcpp/type_support_decl.hpp @@ -0,0 +1,62 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TYPE_SUPPORT_DECL_HPP_ +#define RCLCPP__TYPE_SUPPORT_DECL_HPP_ + +#include "rosidl_generator_cpp/message_type_support_decl.hpp" +#include "rosidl_generator_cpp/service_type_support_decl.hpp" + +namespace rclcpp +{ +namespace type_support +{ + +const rosidl_message_type_support_t * +get_intra_process_message_msg_type_support(); + +const rosidl_message_type_support_t * +get_parameter_event_msg_type_support(); + +const rosidl_message_type_support_t * +get_set_parameters_result_msg_type_support(); + +const rosidl_message_type_support_t * +get_parameter_descriptor_msg_type_support(); + +const rosidl_message_type_support_t * +get_list_parameters_result_msg_type_support(); + +const rosidl_service_type_support_t * +get_get_parameters_srv_type_support(); + +const rosidl_service_type_support_t * +get_get_parameter_types_srv_type_support(); + +const rosidl_service_type_support_t * +get_set_parameters_srv_type_support(); + +const rosidl_service_type_support_t * +get_list_parameters_srv_type_support(); + +const rosidl_service_type_support_t * +get_describe_parameters_srv_type_support(); + +const rosidl_service_type_support_t * +get_set_parameters_atomically_srv_type_support(); + +} // namespace type_support +} // namespace rclcpp + +#endif // RCLCPP__TYPE_SUPPORT_DECL_HPP_ diff --git a/rclcpp/include/rclcpp/type_support_def.hpp b/rclcpp/include/rclcpp/type_support_def.hpp new file mode 100644 index 0000000..899ab3c --- /dev/null +++ b/rclcpp/include/rclcpp/type_support_def.hpp @@ -0,0 +1,123 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__TYPE_SUPPORT_DEF_HPP_ +#define RCLCPP__TYPE_SUPPORT_DEF_HPP_ + +#include "rclcpp/type_support_decl.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rcl_interfaces/msg/list_parameters_result.hpp" +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcl_interfaces/msg/parameter_event.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rcl_interfaces/srv/describe_parameters.hpp" +#include "rcl_interfaces/srv/get_parameter_types.hpp" +#include "rcl_interfaces/srv/get_parameters.hpp" +#include "rcl_interfaces/srv/list_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters.hpp" +#include "rcl_interfaces/srv/set_parameters_atomically.hpp" +#include "rosidl_generator_cpp/message_type_support.hpp" +#include "rosidl_generator_cpp/service_type_support.hpp" + +const rosidl_message_type_support_t * +rclcpp::type_support::get_intra_process_message_msg_type_support() +{ + return rosidl_generator_cpp::get_message_type_support_handle< + rcl_interfaces::msg::IntraProcessMessage + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_parameter_event_msg_type_support() +{ + return rosidl_generator_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ParameterEvent + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_set_parameters_result_msg_type_support() +{ + return rosidl_generator_cpp::get_message_type_support_handle< + rcl_interfaces::msg::SetParametersResult + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_parameter_descriptor_msg_type_support() +{ + return rosidl_generator_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ParameterDescriptor + >(); +} + +const rosidl_message_type_support_t * +rclcpp::type_support::get_list_parameters_result_msg_type_support() +{ + return rosidl_generator_cpp::get_message_type_support_handle< + rcl_interfaces::msg::ListParametersResult + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_get_parameters_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::GetParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_get_parameter_types_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::GetParameterTypes + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_set_parameters_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::SetParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_list_parameters_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::ListParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_describe_parameters_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::DescribeParameters + >(); +} + +const rosidl_service_type_support_t * +rclcpp::type_support::get_set_parameters_atomically_srv_type_support() +{ + return rosidl_generator_cpp::get_service_type_support_handle< + rcl_interfaces::srv::SetParametersAtomically + >(); +} + +#endif // RCLCPP__TYPE_SUPPORT_DEF_HPP_ diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 520818e..40e3345 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -15,97 +15,14 @@ #ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ #define RCLCPP_RCLCPP_UTILITIES_HPP_ -// TODO(wjwwood): remove -#include - -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Determine if sigaction is available -#if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE -#define HAS_SIGACTION -#endif - -namespace -{ -/// Represent the status of the global interrupt signal. -volatile sig_atomic_t g_signal_status = 0; -/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. -rmw_guard_condition_t * g_sigint_guard_cond_handle = \ - rmw_create_guard_condition(); -/// Condition variable for timed sleep (see sleep_for). -std::condition_variable g_interrupt_condition_variable; -std::atomic g_is_interrupted(false); -/// Mutex for protecting the global condition variable. -std::mutex g_interrupt_mutex; - -#ifdef HAS_SIGACTION -struct sigaction old_action; -#else -void (* old_signal_handler)(int) = 0; -#endif - -/// Handle the interrupt signal. -/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up - * and triggers the interrupt guard condition, so that all global threads managed by rclcpp - * are interrupted. - */ -void -#ifdef HAS_SIGACTION -signal_handler(int signal_value, siginfo_t * siginfo, void * context) -#else -signal_handler(int signal_value) -#endif -{ - // TODO(wjwwood): remove - std::cout << "signal_handler(" << signal_value << ")" << std::endl; -#ifdef HAS_SIGACTION - if (old_action.sa_flags & SA_SIGINFO) { - if (old_action.sa_sigaction != NULL) { - old_action.sa_sigaction(signal_value, siginfo, context); - } - } else { - // *INDENT-OFF* - if ( - old_action.sa_handler != NULL && // Is set - old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN) // Is not ignored - // *INDENT-ON* - { - old_action.sa_handler(signal_value); - } - } -#else - if (old_signal_handler) { - old_signal_handler(signal_value); - } -#endif - g_signal_status = signal_value; - rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); - } - g_is_interrupted.store(true); - g_interrupt_condition_variable.notify_all(); -} -} // namespace +#include "rclcpp/visibility_control.hpp" +#include "rmw/macros.h" +#include "rmw/rmw.h" namespace rclcpp { - -RMW_THREAD_LOCAL size_t thread_id = 0; - namespace utilities { @@ -114,113 +31,37 @@ namespace utilities * \param[in] argc Number of arguments. * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp. */ +RCLCPP_PUBLIC void -init(int argc, char * argv[]) -{ - (void)argc; - (void)argv; - g_is_interrupted.store(false); - rmw_ret_t status = rmw_init(); - if (status != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to initialize rmw implementation: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } -#ifdef HAS_SIGACTION - struct sigaction action; - memset(&action, 0, sizeof(action)); - sigemptyset(&action.sa_mask); - action.sa_sigaction = ::signal_handler; - action.sa_flags = SA_SIGINFO; - ssize_t ret = sigaction(SIGINT, &action, &old_action); - if (ret == -1) -#else - ::old_signal_handler = std::signal(SIGINT, ::signal_handler); - if (::old_signal_handler == SIG_ERR) -#endif - { - const size_t error_length = 1024; - char error_string[error_length]; -#ifndef _WIN32 - auto rc = strerror_r(errno, error_string, error_length); - if (rc) { - // *INDENT-OFF* - throw std::runtime_error( - "Failed to set SIGINT signal handler: (" + std::to_string(errno) + - ") unable to retrieve error string"); - // *INDENT-ON* - } - -#else - strerror_s(error_string, error_length, errno); -#endif - // *INDENT-OFF* - throw std::runtime_error( - std::string("Failed to set SIGINT signal handler: (" + std::to_string(errno) + ")") + - error_string); - // *INDENT-ON* - } -} +init(int argc, char * argv[]); /// Check rclcpp's status. // \return True if SIGINT hasn't fired yet, false otherwise. +RCLCPP_PUBLIC bool -ok() -{ - return ::g_signal_status == 0; -} +ok(); /// Notify the signal handler and rmw that rclcpp is shutting down. +RCLCPP_PUBLIC void -shutdown() -{ - g_signal_status = SIGINT; - rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to trigger guard condition: %s\n", rmw_get_error_string_safe()); - } - g_is_interrupted.store(true); - g_interrupt_condition_variable.notify_all(); -} +shutdown(); /// Get a handle to the rmw guard condition that manages the signal handler. +RCLCPP_PUBLIC rmw_guard_condition_t * -get_global_sigint_guard_condition() -{ - return ::g_sigint_guard_cond_handle; -} +get_global_sigint_guard_condition(); /// Use the global condition variable to block for the specified amount of time. /** * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. * \return True if the condition variable did not timeout. */ +RCLCPP_PUBLIC bool -sleep_for(const std::chrono::nanoseconds & nanoseconds) -{ - // TODO: determine if posix's nanosleep(2) is more efficient here - std::chrono::nanoseconds time_left = nanoseconds; - { - std::unique_lock lock(::g_interrupt_mutex); - auto start = std::chrono::steady_clock::now(); - ::g_interrupt_condition_variable.wait_for(lock, nanoseconds); - time_left -= std::chrono::steady_clock::now() - start; - } - if (time_left > std::chrono::nanoseconds::zero() && !g_is_interrupted) { - return sleep_for(time_left); - } - // Return true if the timeout elapsed successfully, otherwise false. - return !g_is_interrupted; -} +sleep_for(const std::chrono::nanoseconds & nanoseconds); -} /* namespace utilities */ -} /* namespace rclcpp */ - -#ifdef HAS_SIGACTION -#undef HAS_SIGACTION -#endif +} // namespace utilities +} // namespace rclcpp #endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */ diff --git a/rclcpp/include/rclcpp/visibility_control.hpp b/rclcpp/include/rclcpp/visibility_control.hpp new file mode 100644 index 0000000..747ab10 --- /dev/null +++ b/rclcpp/include/rclcpp/visibility_control.hpp @@ -0,0 +1,83 @@ +// Copyright 2015 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RCLCPP__VISIBILITY_CONTROL_HPP_ +#define RCLCPP__VISIBILITY_CONTROL_HPP_ + +#include "rmw/rmw.h" + +#if !defined(RCLCPP_BUILDING_LIBRARY) +// The following header is necessary inorder to get the correct rmw +// implementation specific message symbols. +// Any library or executable using librclcpp must include the header. +// However, librclcpp must not include it. +#include "rclcpp/type_support_def.hpp" + +// Anonymous namespace to prevent duplicate symbols across compile units. +namespace +{ + +// This call to an rmw function is used to force the Linux linker to include +// the rmw implementation library in the user's executable. +// If this is not done, then only librclcpp is missing rmw symbols and when +// linking the next library or exectuable the linker will discard the rmw +// implementation shared library as unused. +// On OS X this isn't an issue because linking is done, by default, in a flat +// namespace, so when linking something that uses librclcpp, it will try to +// resolve the rclcpp symbols each time, taking them from the rmw implementation +// when it is available and not discarding it as unused during the link step. +static const char * __rmw_identifier = rmw_get_implementation_identifier(); + +} // namespace +#endif // !defined(RCLCPP_BUILDING_LIBRARY) + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RCLCPP_EXPORT __attribute__ ((dllexport)) + #define RCLCPP_IMPORT __attribute__ ((dllimport)) + #else + #define RCLCPP_EXPORT __declspec(dllexport) + #define RCLCPP_IMPORT __declspec(dllimport) + #endif + #ifdef RCLCPP_BUILDING_LIBRARY + #define RCLCPP_PUBLIC RCLCPP_EXPORT + #else + #define RCLCPP_PUBLIC RCLCPP_IMPORT + #endif + #define RCLCPP_PUBLIC_TYPE RCLCPP_PUBLIC + #define RCLCPP_LOCAL +#else + #define RCLCPP_EXPORT __attribute__ ((visibility("default"))) + #define RCLCPP_IMPORT + #if __GNUC__ >= 4 + #define RCLCPP_PUBLIC __attribute__ ((visibility("default"))) + #define RCLCPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCLCPP_PUBLIC + #define RCLCPP_LOCAL + #endif + #define RCLCPP_PUBLIC_TYPE +#endif + +#endif // RCLCPP__VISIBILITY_CONTROL_HPP_ diff --git a/rclcpp/package.xml b/rclcpp/package.xml index afae9eb..2ffce3e 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -11,7 +11,11 @@ rmw rcl_interfaces + + rmw_implementation + rosidl_generator_cpp rcl_interfaces + rosidl_generator_cpp ament_cmake_gtest ament_lint_auto diff --git a/rclcpp/src/rclcpp/any_executable.cpp b/rclcpp/src/rclcpp/any_executable.cpp index 1bdddb8..0566140 100644 --- a/rclcpp/src/rclcpp/any_executable.cpp +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -12,37 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_ -#define RCLCPP_RCLCPP_ANY_EXECUTABLE_HPP_ +#include "rclcpp/any_executable.hpp" -#include +using rclcpp::executor::AnyExecutable; -#include -#include - -namespace rclcpp -{ -namespace executor -{ - -struct AnyExecutable -{ - RCLCPP_SMART_PTR_DEFINITIONS(AnyExecutable); - AnyExecutable() - : subscription(0), timer(0), callback_group(0), node(0) - {} - // Only one of the following pointers will be set. - rclcpp::subscription::SubscriptionBase::SharedPtr subscription; - rclcpp::subscription::SubscriptionBase::SharedPtr subscription_intra_process; - rclcpp::timer::TimerBase::SharedPtr timer; - rclcpp::service::ServiceBase::SharedPtr service; - rclcpp::client::ClientBase::SharedPtr client; - // These are used to keep the scope on the containing items - rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; - rclcpp::node::Node::SharedPtr node; -}; - -} /* executor */ -} /* rclcpp */ - -#endif +AnyExecutable::AnyExecutable() +: subscription(nullptr), + subscription_intra_process(nullptr), + timer(nullptr), + service(nullptr), + client(nullptr), + callback_group(nullptr), + node(nullptr) +{} diff --git a/rclcpp/src/rclcpp/callback_group.cpp b/rclcpp/src/rclcpp/callback_group.cpp index ca050fa..2555973 100644 --- a/rclcpp/src/rclcpp/callback_group.cpp +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,119 +12,74 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ -#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ +#include "rclcpp/callback_group.hpp" -#include -#include #include -#include -#include -#include -#include +using rclcpp::callback_group::CallbackGroup; +using rclcpp::callback_group::CallbackGroupType; -namespace rclcpp +CallbackGroup::CallbackGroup(CallbackGroupType group_type) +: type_(group_type), can_be_taken_from_(true) +{} + +const std::vector & +CallbackGroup::get_subscription_ptrs() const { + return subscription_ptrs_; +} -// Forward declarations for friend statement in class CallbackGroup -namespace node +const std::vector & +CallbackGroup::get_timer_ptrs() const { -class Node; -} // namespace node + return timer_ptrs_; +} -namespace callback_group +const std::vector & +CallbackGroup::get_service_ptrs() const { + return service_ptrs_; +} -enum class CallbackGroupType +const std::vector & +CallbackGroup::get_client_ptrs() const { - MutuallyExclusive, - Reentrant -}; + return client_ptrs_; +} -class CallbackGroup +std::atomic_bool & +CallbackGroup::can_be_taken_from() { - friend class rclcpp::node::Node; + return can_be_taken_from_; +} -public: - RCLCPP_SMART_PTR_DEFINITIONS(CallbackGroup); +const CallbackGroupType & +CallbackGroup::type() const +{ + return type_; +} - CallbackGroup(CallbackGroupType group_type) - : type_(group_type), can_be_taken_from_(true) - {} +void +CallbackGroup::add_subscription( + const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr) +{ + subscription_ptrs_.push_back(subscription_ptr); +} - const std::vector & - get_subscription_ptrs() const - { - return subscription_ptrs_; - } +void +CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr) +{ + timer_ptrs_.push_back(timer_ptr); +} - const std::vector & - get_timer_ptrs() const - { - return timer_ptrs_; - } +void +CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr) +{ + service_ptrs_.push_back(service_ptr); +} - const std::vector & - get_service_ptrs() const - { - return service_ptrs_; - } - - const std::vector & - get_client_ptrs() const - { - return client_ptrs_; - } - - std::atomic_bool & can_be_taken_from() - { - return can_be_taken_from_; - } - - const CallbackGroupType & type() const - { - return type_; - } - -private: - RCLCPP_DISABLE_COPY(CallbackGroup); - - void - add_subscription( - const subscription::SubscriptionBase::SharedPtr subscription_ptr) - { - subscription_ptrs_.push_back(subscription_ptr); - } - - void - add_timer(const timer::TimerBase::SharedPtr timer_ptr) - { - timer_ptrs_.push_back(timer_ptr); - } - - void - add_service(const service::ServiceBase::SharedPtr service_ptr) - { - service_ptrs_.push_back(service_ptr); - } - - void - add_client(const client::ClientBase::SharedPtr client_ptr) - { - client_ptrs_.push_back(client_ptr); - } - - CallbackGroupType type_; - std::vector subscription_ptrs_; - std::vector timer_ptrs_; - std::vector service_ptrs_; - std::vector client_ptrs_; - std::atomic_bool can_be_taken_from_; - -}; - -} /* namespace callback_group */ -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */ +void +CallbackGroup::add_client(const rclcpp::client::ClientBase::SharedPtr client_ptr) +{ + client_ptrs_.push_back(client_ptr); +} diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 7037363..29aa62a 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,153 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_CLIENT_HPP_ -#define RCLCPP_RCLCPP_CLIENT_HPP_ +#include "rclcpp/client.hpp" -#include -#include -#include -#include -#include -#include +#include +#include -#include -#include +#include "rmw/rmw.h" -#include -#include +using rclcpp::client::ClientBase; -namespace rclcpp +ClientBase::ClientBase( + std::shared_ptr node_handle, + rmw_client_t * client_handle, + const std::string & service_name) +: node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) +{} + +ClientBase::~ClientBase() { - -namespace client -{ - -class ClientBase -{ - -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ClientBase); - - ClientBase( - std::shared_ptr node_handle, - rmw_client_t * client_handle, - const std::string & service_name) - : node_handle_(node_handle), client_handle_(client_handle), service_name_(service_name) - {} - - virtual ~ClientBase() - { - if (client_handle_) { - if (rmw_destroy_client(client_handle_) != RMW_RET_OK) { - fprintf(stderr, - "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); - } + if (client_handle_) { + if (rmw_destroy_client(client_handle_) != RMW_RET_OK) { + fprintf(stderr, + "Error in destruction of rmw client handle: %s\n", rmw_get_error_string_safe()); } } +} - const std::string & get_service_name() const - { - return this->service_name_; - } - - const rmw_client_t * get_client_handle() const - { - return this->client_handle_; - } - - virtual std::shared_ptr create_response() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_response( - std::shared_ptr & request_header, std::shared_ptr & response) = 0; - -private: - RCLCPP_DISABLE_COPY(ClientBase); - - std::shared_ptr node_handle_; - - rmw_client_t * client_handle_; - std::string service_name_; - -}; - -template -class Client : public ClientBase +const std::string & +ClientBase::get_service_name() const { -public: - using Promise = std::promise; - using SharedPromise = std::shared_ptr; - using SharedFuture = std::shared_future; + return this->service_name_; +} - using CallbackType = std::function; - - RCLCPP_SMART_PTR_DEFINITIONS(Client); - - Client( - std::shared_ptr node_handle, - rmw_client_t * client_handle, - const std::string & service_name) - : ClientBase(node_handle, client_handle, service_name) - {} - - std::shared_ptr create_response() - { - return std::shared_ptr(new typename ServiceT::Response()); - } - - std::shared_ptr create_request_header() - { - // TODO(wjwwood): This should probably use rmw_request_id's allocator. - // (since it is a C type) - return std::shared_ptr(new rmw_request_id_t); - } - - void handle_response(std::shared_ptr & request_header, std::shared_ptr & response) - { - auto typed_request_header = std::static_pointer_cast(request_header); - auto typed_response = std::static_pointer_cast(response); - int64_t sequence_number = typed_request_header->sequence_number; - // TODO this must check if the sequence_number is valid otherwise the call_promise will be null - auto tuple = this->pending_requests_[sequence_number]; - auto call_promise = std::get<0>(tuple); - auto callback = std::get<1>(tuple); - auto future = std::get<2>(tuple); - this->pending_requests_.erase(sequence_number); - call_promise->set_value(typed_response); - callback(future); - } - - SharedFuture async_send_request( - typename ServiceT::Request::SharedPtr request) - { - return async_send_request(request, [](SharedFuture) {}); - } - - SharedFuture async_send_request( - typename ServiceT::Request::SharedPtr request, - CallbackType cb) - { - int64_t sequence_number; - if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to send request: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - SharedPromise call_promise = std::make_shared(); - SharedFuture f(call_promise->get_future()); - pending_requests_[sequence_number] = std::make_tuple(call_promise, cb, f); - return f; - } - -private: - RCLCPP_DISABLE_COPY(Client); - - std::map> pending_requests_; -}; - -} /* namespace client */ -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */ +const rmw_client_t * +ClientBase::get_client_handle() const +{ + return this->client_handle_; +} diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index 1c56cbb..a5f3c17 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,69 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_ -#define RCLCPP_RCLCPP_CONTEXT_HPP_ +#include "rclcpp/context.hpp" -#include +using rclcpp::context::Context; -#include - -#include -#include -#include -#include -#include - -#include - -namespace rclcpp -{ - -namespace context -{ - -class Context -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(Context); - - Context() {} - - template - std::shared_ptr - get_sub_context(Args && ... args) - { - std::lock_guard lock(mutex_); - - std::type_index type_i(typeid(SubContext)); - std::shared_ptr sub_context; - auto it = sub_contexts_.find(type_i); - if (it == sub_contexts_.end()) { - // It doesn't exist yet, make it - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - sub_context = std::shared_ptr( - new SubContext(std::forward(args) ...), - [] (SubContext * sub_context_ptr) { - delete sub_context_ptr; - }); - // *INDENT-ON* - sub_contexts_[type_i] = sub_context; - } else { - // It exists, get it out and cast it. - sub_context = std::static_pointer_cast(it->second); - } - return sub_context; - } - -private: - RCLCPP_DISABLE_COPY(Context); - - std::unordered_map> sub_contexts_; - std::mutex mutex_; - -}; - -} /* namespace context */ -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */ +Context::Context() {} diff --git a/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp/src/rclcpp/contexts/default_context.cpp index c10225e..53c6c9f 100644 --- a/rclcpp/src/rclcpp/contexts/default_context.cpp +++ b/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,36 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ -#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ +#include "rclcpp/contexts/default_context.hpp" -#include +using rclcpp::contexts::default_context::DefaultContext; -namespace rclcpp -{ -namespace contexts -{ -namespace default_context -{ - -class DefaultContext : public rclcpp::context::Context -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(DefaultContext); - - DefaultContext() {} - -}; +DefaultContext::DefaultContext() +{} DefaultContext::SharedPtr -get_global_default_context() +rclcpp::contexts::default_context::get_global_default_context() { static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); return default_context; } - -} // namespace default_context -} // namespace contexts -} // namespace rclcpp - -#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */ diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index cc3fe42..4ee8ce0 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,605 +12,507 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__EXECUTOR_HPP_ -#define RCLCPP__EXECUTOR_HPP_ - -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/executor.hpp" #include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rclcpp/any_executable.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/memory_strategy.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/utilities.hpp" +using rclcpp::executor::AnyExecutable; +using rclcpp::executor::Executor; -namespace rclcpp +Executor::Executor(memory_strategy::MemoryStrategy::SharedPtr ms) +: interrupt_guard_condition_(rmw_create_guard_condition()), + memory_strategy_(ms) { -namespace executor +} + +Executor::~Executor() { - -/// Coordinate the order and timing of available communication tasks. -/** - * Executor provides spin functions (including spin_node_once and spin_some). - * It coordinates the nodes and callback groups by looking for available work and completing it, - * based on the threading or concurrency scheme provided by the subclass implementation. - * An example of available work is executing a subscription callback, or a timer callback. - * The executor structure allows for a decoupling of the communication graph and the execution - * model. - * See SingleThreadedExecutor and MultiThreadedExecutor for examples of execution paradigms. - */ -class Executor -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Executor); - - /// Default constructor. - // \param[in] ms The memory strategy to be used with this executor. - explicit Executor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : interrupt_guard_condition_(rmw_create_guard_condition()), - memory_strategy_(ms) - { - } - - /// Default destructor. - virtual ~Executor() - { - // Try to deallocate the interrupt guard condition. - if (interrupt_guard_condition_ != nullptr) { - rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - fprintf(stderr, - "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); - } + // Try to deallocate the interrupt guard condition. + if (interrupt_guard_condition_ != nullptr) { + rmw_ret_t status = rmw_destroy_guard_condition(interrupt_guard_condition_); + if (status != RMW_RET_OK) { + fprintf(stderr, + "[rclcpp::error] failed to destroy guard condition: %s\n", rmw_get_error_string_safe()); } } +} - /// Do work periodically as it becomes available to us. Blocking call, may block indefinitely. - // It is up to the implementation of Executor to implement spin. - virtual void spin() = 0; - - /// Add a node to the executor. - /** - * An executor can have zero or more nodes which provide work during `spin` functions. - * \param[in] node_ptr Shared pointer to the node to be added. - * \param[in] notify True to trigger the interrupt guard condition during this function. If - * the executor is blocked at the rmw layer while waiting for work and it is notified that a new - * node was added, it will wake up. - */ - virtual void - add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true) - { - // Check to ensure node not already added - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (node == node_ptr) { - // TODO(jacquelinekay): Use a different error here? - throw std::runtime_error("Cannot add node to executor, node already added."); - } +void +Executor::add_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) +{ + // Check to ensure node not already added + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (node == node_ptr) { + // TODO(jacquelinekay): Use a different error here? + throw std::runtime_error("Cannot add node to executor, node already added."); } - weak_nodes_.push_back(node_ptr); - if (notify) { - // Interrupt waiting to handle new node + } + weak_nodes_.push_back(node_ptr); + if (notify) { + // Interrupt waiting to handle new node + rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); + if (status != RMW_RET_OK) { + throw std::runtime_error(rmw_get_error_string_safe()); + } + } +} + +void +Executor::remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify) +{ + bool node_removed = false; + weak_nodes_.erase( + std::remove_if( + weak_nodes_.begin(), weak_nodes_.end(), + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + [&](std::weak_ptr & i) + { + bool matched = (i.lock() == node_ptr); + node_removed |= matched; + return matched; + } + // *INDENT-ON* + ) + ); + if (notify) { + // If the node was matched and removed, interrupt waiting + if (node_removed) { rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); if (status != RMW_RET_OK) { throw std::runtime_error(rmw_get_error_string_safe()); } } } +} - /// Remove a node from the executor. - /** - * \param[in] node_ptr Shared pointer to the node to remove. - * \param[in] notify True to trigger the interrupt guard condition and wake up the executor. - * This is useful if the last node was removed from the executor while the executor was blocked - * waiting for work in another thread, because otherwise the executor would never be notified. - */ - virtual void - remove_node(rclcpp::node::Node::SharedPtr node_ptr, bool notify = true) +void +Executor::spin_node_once_nanoseconds( + rclcpp::node::Node::SharedPtr node, + std::chrono::nanoseconds timeout) +{ + this->add_node(node, false); + // non-blocking = true + auto any_exec = get_next_executable(timeout); + if (any_exec) { + execute_any_executable(any_exec); + } + this->remove_node(node, false); +} + +void +Executor::spin_node_some(rclcpp::node::Node::SharedPtr node) +{ + this->add_node(node, false); + spin_some(); + this->remove_node(node, false); +} + +void +Executor::spin_some() +{ + while (AnyExecutable::SharedPtr any_exec = + get_next_executable(std::chrono::milliseconds::zero())) { - bool node_removed = false; + execute_any_executable(any_exec); + } +} + +void +Executor::set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy) +{ + if (memory_strategy == nullptr) { + throw std::runtime_error("Received NULL memory strategy in executor."); + } + memory_strategy_ = memory_strategy; +} + +void +Executor::execute_any_executable(AnyExecutable::SharedPtr any_exec) +{ + if (!any_exec) { + return; + } + if (any_exec->timer) { + execute_timer(any_exec->timer); + } + if (any_exec->subscription) { + execute_subscription(any_exec->subscription); + } + if (any_exec->subscription_intra_process) { + execute_intra_process_subscription(any_exec->subscription_intra_process); + } + if (any_exec->service) { + execute_service(any_exec->service); + } + if (any_exec->client) { + execute_client(any_exec->client); + } + // Reset the callback_group, regardless of type + any_exec->callback_group->can_be_taken_from().store(true); + // Wake the wait, because it may need to be recalculated or work that + // was previously blocked is now available. + rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); + if (status != RMW_RET_OK) { + throw std::runtime_error(rmw_get_error_string_safe()); + } +} + +void +Executor::execute_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription) +{ + std::shared_ptr message = subscription->create_message(); + bool taken = false; + rmw_message_info_t message_info; + auto ret = + rmw_take_with_info(subscription->get_subscription_handle(), + message.get(), &taken, &message_info); + if (ret == RMW_RET_OK) { + if (taken) { + message_info.from_intra_process = false; + subscription->handle_message(message, message_info); + } + } else { + fprintf(stderr, + "[rclcpp::error] take failed for subscription on topic '%s': %s\n", + subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); + } + subscription->return_message(message); +} + +void +Executor::execute_intra_process_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription) +{ + rcl_interfaces::msg::IntraProcessMessage ipm; + bool taken = false; + rmw_message_info_t message_info; + rmw_ret_t status = rmw_take_with_info( + subscription->get_intra_process_subscription_handle(), + &ipm, + &taken, + &message_info); + if (status == RMW_RET_OK) { + if (taken) { + message_info.from_intra_process = true; + subscription->handle_intra_process_message(ipm, message_info); + } + } else { + fprintf(stderr, + "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", + subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); + } +} + +void +Executor::execute_timer( + rclcpp::timer::TimerBase::SharedPtr timer) +{ + timer->execute_callback(); +} + +void +Executor::execute_service( + rclcpp::service::ServiceBase::SharedPtr service) +{ + std::shared_ptr request_header = service->create_request_header(); + std::shared_ptr request = service->create_request(); + bool taken = false; + rmw_ret_t status = rmw_take_request( + service->get_service_handle(), + request_header.get(), + request.get(), + &taken); + if (status == RMW_RET_OK) { + if (taken) { + service->handle_request(request_header, request); + } + } else { + fprintf(stderr, + "[rclcpp::error] take request failed for server of service '%s': %s\n", + service->get_service_name().c_str(), rmw_get_error_string_safe()); + } +} + +void +Executor::execute_client( + rclcpp::client::ClientBase::SharedPtr client) +{ + std::shared_ptr request_header = client->create_request_header(); + std::shared_ptr response = client->create_response(); + bool taken = false; + rmw_ret_t status = rmw_take_response( + client->get_client_handle(), + request_header.get(), + response.get(), + &taken); + if (status == RMW_RET_OK) { + if (taken) { + client->handle_response(request_header, response); + } + } else { + fprintf(stderr, + "[rclcpp::error] take response failed for client of service '%s': %s\n", + client->get_service_name().c_str(), rmw_get_error_string_safe()); + } +} + +void +Executor::wait_for_work(std::chrono::nanoseconds timeout) +{ + memory_strategy_->clear_active_entities(); + + // Collect the subscriptions and timers to be waited on + bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); + + // Clean up any invalid nodes, if they were detected + if (has_invalid_weak_nodes) { weak_nodes_.erase( - std::remove_if( + remove_if( weak_nodes_.begin(), weak_nodes_.end(), - [&](std::weak_ptr & i) - { - bool matched = (i.lock() == node_ptr); - node_removed |= matched; - return matched; - })); - if (notify) { - // If the node was matched and removed, interrupt waiting - if (node_removed) { - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - } - } - } - - /// Add a node to executor, execute the next available unit of work, and remove the node. - /** - * \param[in] node Shared pointer to the node to add. - * \param[in] timeout How long to wait for work to become available. Negative values cause - * spin_node_once to block indefinitely (the default behavior). A timeout of 0 causes this - * function to be non-blocking. - */ - template - void spin_node_once(rclcpp::node::Node::SharedPtr node, - std::chrono::duration timeout = std::chrono::duration(-1)) - { - this->add_node(node, false); - // non-blocking = true - auto any_exec = get_next_executable(timeout); - if (any_exec) { - execute_any_executable(any_exec); - } - this->remove_node(node, false); - } - - /// Add a node, complete all immediately available work, and remove the node. - /** - * \param[in] node Shared pointer to the node to add. - */ - void spin_node_some(rclcpp::node::Node::SharedPtr node) - { - this->add_node(node, false); - spin_some(); - this->remove_node(node, false); - } - - /// Complete all available queued work without blocking. - /** - * This function can be overridden. The default implementation is suitable for a - * single-threaded model of execution. - * Adding subscriptions, timers, services, etc. with blocking callbacks will cause this function - * to block (which may have unintended consequences). - */ - virtual void spin_some() - { - while (AnyExecutable::SharedPtr any_exec = - get_next_executable(std::chrono::milliseconds::zero())) - { - execute_any_executable(any_exec); - } - } - - /// Support dynamic switching of the memory strategy. - /** - * Switching the memory strategy while the executor is spinning in another threading could have - * unintended consequences. - * \param[in] memory_strategy Shared pointer to the memory strategy to set. - */ - void - set_memory_strategy(memory_strategy::MemoryStrategy::SharedPtr memory_strategy) - { - if (memory_strategy == nullptr) { - throw std::runtime_error("Received NULL memory strategy in executor."); - } - memory_strategy_ = memory_strategy; - } - -protected: - /// Find the next available executable and do the work associated with it. - /** \param[in] any_exec Union structure that can hold any executable type (timer, subscription, - * service, client). - */ - void - execute_any_executable(AnyExecutable::SharedPtr any_exec) - { - if (!any_exec) { - return; - } - if (any_exec->timer) { - execute_timer(any_exec->timer); - } - if (any_exec->subscription) { - execute_subscription(any_exec->subscription); - } - if (any_exec->subscription_intra_process) { - execute_intra_process_subscription(any_exec->subscription_intra_process); - } - if (any_exec->service) { - execute_service(any_exec->service); - } - if (any_exec->client) { - execute_client(any_exec->client); - } - // Reset the callback_group, regardless of type - any_exec->callback_group->can_be_taken_from().store(true); - // Wake the wait, because it may need to be recalculated or work that - // was previously blocked is now available. - rmw_ret_t status = rmw_trigger_guard_condition(interrupt_guard_condition_); - if (status != RMW_RET_OK) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - } - - static void - execute_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr subscription) - { - std::shared_ptr message = subscription->create_message(); - bool taken = false; - rmw_message_info_t message_info; - auto ret = - rmw_take_with_info(subscription->get_subscription_handle(), - message.get(), &taken, &message_info); - if (ret == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = false; - subscription->handle_message(message, message_info); - } - } else { - fprintf(stderr, - "[rclcpp::error] take failed for subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); - } - subscription->return_message(message); - } - - static void - execute_intra_process_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr subscription) - { - rcl_interfaces::msg::IntraProcessMessage ipm; - bool taken = false; - rmw_message_info_t message_info; - rmw_ret_t status = rmw_take_with_info( - subscription->get_intra_process_subscription_handle(), - &ipm, - &taken, - &message_info); - if (status == RMW_RET_OK) { - if (taken) { - message_info.from_intra_process = true; - subscription->handle_intra_process_message(ipm, message_info); - } - } else { - fprintf(stderr, - "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n", - subscription->get_topic_name().c_str(), rmw_get_error_string_safe()); - } - } - - static void - execute_timer( - rclcpp::timer::TimerBase::SharedPtr timer) - { - timer->execute_callback(); - } - - static void - execute_service( - rclcpp::service::ServiceBase::SharedPtr service) - { - std::shared_ptr request_header = service->create_request_header(); - std::shared_ptr request = service->create_request(); - bool taken = false; - rmw_ret_t status = rmw_take_request( - service->get_service_handle(), - request_header.get(), - request.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { - service->handle_request(request_header, request); - } - } else { - fprintf(stderr, - "[rclcpp::error] take request failed for server of service '%s': %s\n", - service->get_service_name().c_str(), rmw_get_error_string_safe()); - } - } - - static void - execute_client( - rclcpp::client::ClientBase::SharedPtr client) - { - std::shared_ptr request_header = client->create_request_header(); - std::shared_ptr response = client->create_response(); - bool taken = false; - rmw_ret_t status = rmw_take_response( - client->get_client_handle(), - request_header.get(), - response.get(), - &taken); - if (status == RMW_RET_OK) { - if (taken) { - client->handle_response(request_header, response); - } - } else { - fprintf(stderr, - "[rclcpp::error] take response failed for client of service '%s': %s\n", - client->get_service_name().c_str(), rmw_get_error_string_safe()); - } - } - -/*** Populate class storage from stored weak node pointers and wait. ***/ - - template - void - wait_for_work(std::chrono::duration timeout = std::chrono::duration(-1)) - { - memory_strategy_->clear_active_entities(); - - // Collect the subscriptions and timers to be waited on - bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_); - - // Clean up any invalid nodes, if they were detected - if (has_invalid_weak_nodes) { - weak_nodes_.erase( - remove_if(weak_nodes_.begin(), weak_nodes_.end(), + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) [](std::weak_ptr i) - { - return i.expired(); - })); - } - - // Use the number of subscriptions to allocate memory in the handles - rmw_subscriptions_t subscriber_handles; - subscriber_handles.subscriber_count = - memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers); - - rmw_services_t service_handles; - service_handles.service_count = - memory_strategy_->fill_service_handles(service_handles.services); - - rmw_clients_t client_handles; - client_handles.client_count = - memory_strategy_->fill_client_handles(client_handles.clients); - - // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond, - // and one for the executor's guard cond (interrupt_guard_condition_) - size_t number_of_guard_conds = 2; - rmw_guard_conditions_t guard_condition_handles; - guard_condition_handles.guard_condition_count = number_of_guard_conds; - guard_condition_handles.guard_conditions = static_cast(guard_cond_handles_.data()); - if (guard_condition_handles.guard_conditions == NULL && - number_of_guard_conds > 0) - { - // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? - throw std::runtime_error("Could not malloc for guard condition pointers."); - } - // Put the global ctrl-c guard condition in - assert(guard_condition_handles.guard_condition_count > 1); - guard_condition_handles.guard_conditions[0] = \ - rclcpp::utilities::get_global_sigint_guard_condition()->data; - // Put the executor's guard condition in - guard_condition_handles.guard_conditions[1] = \ - interrupt_guard_condition_->data; - - rmw_time_t * wait_timeout = NULL; - rmw_time_t rmw_timeout; - - auto next_timer_duration = get_earliest_timer(); - // If the next timer timeout must preempt the requested timeout - // or if the requested timeout blocks forever, and there exists a valid timer, - // replace the requested timeout with the next timeout. - bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero(); - if ((next_timer_duration < timeout || - timeout < std::chrono::duration::zero()) && has_valid_timer) - { - rmw_timeout.sec = - std::chrono::duration_cast(next_timer_duration).count(); - rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } else if (timeout >= std::chrono::duration::zero()) { - // Convert timeout representation to rmw_time - rmw_timeout.sec = std::chrono::duration_cast(timeout).count(); - rmw_timeout.nsec = std::chrono::duration_cast(timeout).count() % - (1000 * 1000 * 1000); - wait_timeout = &rmw_timeout; - } - - // Now wait on the waitable subscriptions and timers - rmw_ret_t status = rmw_wait( - &subscriber_handles, - &guard_condition_handles, - &service_handles, - &client_handles, - wait_timeout); - if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { - throw std::runtime_error(rmw_get_error_string_safe()); - } - // If ctrl-c guard condition, return directly - if (guard_condition_handles.guard_conditions[0] != 0) { - // Make sure to free or clean memory - memory_strategy_->clear_handles(); - return; - } - - memory_strategy_->revalidate_handles(); + { + return i.expired(); + } + // *INDENT-ON* + ) + ); } -/******************************/ - rclcpp::node::Node::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) + // Use the number of subscriptions to allocate memory in the handles + rmw_subscriptions_t subscriber_handles; + subscriber_handles.subscriber_count = + memory_strategy_->fill_subscriber_handles(subscriber_handles.subscribers); + + rmw_services_t service_handles; + service_handles.service_count = + memory_strategy_->fill_service_handles(service_handles.services); + + rmw_clients_t client_handles; + client_handles.client_count = + memory_strategy_->fill_client_handles(client_handles.clients); + + // The number of guard conditions is fixed at 2: 1 for the ctrl-c guard cond, + // and one for the executor's guard cond (interrupt_guard_condition_) + size_t number_of_guard_conds = 2; + rmw_guard_conditions_t guard_condition_handles; + guard_condition_handles.guard_condition_count = number_of_guard_conds; + guard_condition_handles.guard_conditions = static_cast(guard_cond_handles_.data()); + if (guard_condition_handles.guard_conditions == NULL && + number_of_guard_conds > 0) { - if (!group) { - return rclcpp::node::Node::SharedPtr(); - } - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; - } - } - } + // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? + throw std::runtime_error("Could not malloc for guard condition pointers."); + } + // Put the global ctrl-c guard condition in + assert(guard_condition_handles.guard_condition_count > 1); + guard_condition_handles.guard_conditions[0] = \ + rclcpp::utilities::get_global_sigint_guard_condition()->data; + // Put the executor's guard condition in + guard_condition_handles.guard_conditions[1] = \ + interrupt_guard_condition_->data; + + rmw_time_t * wait_timeout = NULL; + rmw_time_t rmw_timeout; + + auto next_timer_duration = get_earliest_timer(); + // If the next timer timeout must preempt the requested timeout + // or if the requested timeout blocks forever, and there exists a valid timer, + // replace the requested timeout with the next timeout. + bool has_valid_timer = next_timer_duration >= std::chrono::nanoseconds::zero(); + if ((next_timer_duration < timeout || + timeout < std::chrono::nanoseconds::zero()) && has_valid_timer) + { + rmw_timeout.sec = + std::chrono::duration_cast(next_timer_duration).count(); + rmw_timeout.nsec = next_timer_duration.count() % (1000 * 1000 * 1000); + wait_timeout = &rmw_timeout; + } else if (timeout >= std::chrono::nanoseconds::zero()) { + // Convert timeout representation to rmw_time + rmw_timeout.sec = std::chrono::duration_cast(timeout).count(); + rmw_timeout.nsec = std::chrono::duration_cast(timeout).count() % + (1000 * 1000 * 1000); + wait_timeout = &rmw_timeout; + } + + // Now wait on the waitable subscriptions and timers + rmw_ret_t status = rmw_wait( + &subscriber_handles, + &guard_condition_handles, + &service_handles, + &client_handles, + wait_timeout); + if (status != RMW_RET_OK && status != RMW_RET_TIMEOUT) { + throw std::runtime_error(rmw_get_error_string_safe()); + } + // If ctrl-c guard condition, return directly + if (guard_condition_handles.guard_conditions[0] != 0) { + // Make sure to free or clean memory + memory_strategy_->clear_handles(); + return; + } + + memory_strategy_->revalidate_handles(); +} + +rclcpp::node::Node::SharedPtr +Executor::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + if (!group) { return rclcpp::node::Node::SharedPtr(); } + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto callback_group = weak_group.lock(); + if (callback_group == group) { + return node; + } + } + } + return rclcpp::node::Node::SharedPtr(); +} - rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_timer( - rclcpp::timer::TimerBase::SharedPtr timer) - { - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { +rclcpp::callback_group::CallbackGroup::SharedPtr +Executor::get_group_by_timer(rclcpp::timer::TimerBase::SharedPtr timer) +{ + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_timer : group->get_timer_ptrs()) { - auto t = weak_timer.lock(); - if (t == timer) { - return group; - } + for (auto & weak_timer : group->get_timer_ptrs()) { + auto t = weak_timer.lock(); + if (t == timer) { + return group; } } } - return rclcpp::callback_group::CallbackGroup::SharedPtr(); } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); +} - void - get_next_timer(AnyExecutable::SharedPtr any_exec) - { - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { +void +Executor::get_next_timer(AnyExecutable::SharedPtr any_exec) +{ + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group || !group->can_be_taken_from().load()) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - auto timer = timer_ref.lock(); - if (timer && timer->check_and_trigger()) { - any_exec->timer = timer; - any_exec->callback_group = group; - node = get_node_by_group(group); - return; - } + for (auto & timer_ref : group->get_timer_ptrs()) { + auto timer = timer_ref.lock(); + if (timer && timer->check_and_trigger()) { + any_exec->timer = timer; + any_exec->callback_group = group; + node = get_node_by_group(group); + return; } } } } +} - std::chrono::nanoseconds - get_earliest_timer() - { - std::chrono::nanoseconds latest = std::chrono::nanoseconds::max(); - bool timers_empty = true; - for (auto & weak_node : weak_nodes_) { - auto node = weak_node.lock(); - if (!node) { +std::chrono::nanoseconds +Executor::get_earliest_timer() +{ + std::chrono::nanoseconds latest = std::chrono::nanoseconds::max(); + bool timers_empty = true; + for (auto & weak_node : weak_nodes_) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group || !group->can_be_taken_from().load()) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group || !group->can_be_taken_from().load()) { - continue; - } - for (auto & timer_ref : group->get_timer_ptrs()) { - timers_empty = false; - // Check the expected trigger time - auto timer = timer_ref.lock(); - if (timer && timer->time_until_trigger() < latest) { - latest = timer->time_until_trigger(); - } + for (auto & timer_ref : group->get_timer_ptrs()) { + timers_empty = false; + // Check the expected trigger time + auto timer = timer_ref.lock(); + if (timer && timer->time_until_trigger() < latest) { + latest = timer->time_until_trigger(); } } } - if (timers_empty) { - return std::chrono::nanoseconds(-1); - } - return latest; } - - AnyExecutable::SharedPtr - get_next_ready_executable() - { - auto any_exec = memory_strategy_->instantiate_next_executable(); - // Check the timers to see if there are any that are ready, if so return - get_next_timer(any_exec); - if (any_exec->timer) { - return any_exec; - } - // Check the subscriptions to see if there are any that are ready - memory_strategy_->get_next_subscription(any_exec, weak_nodes_); - if (any_exec->subscription || any_exec->subscription_intra_process) { - return any_exec; - } - // Check the services to see if there are any that are ready - memory_strategy_->get_next_service(any_exec, weak_nodes_); - if (any_exec->service) { - return any_exec; - } - // Check the clients to see if there are any that are ready - memory_strategy_->get_next_client(any_exec, weak_nodes_); - if (any_exec->client) { - return any_exec; - } - // If there is no ready executable, return a null ptr - return nullptr; + if (timers_empty) { + return std::chrono::nanoseconds(-1); } + return latest; +} - template - AnyExecutable::SharedPtr - get_next_executable(std::chrono::duration timeout = std::chrono::duration(-1)) - { - // Check to see if there are any subscriptions or timers needing service - // TODO(wjwwood): improve run to run efficiency of this function - auto any_exec = get_next_ready_executable(); - // If there are none - if (!any_exec) { - // Wait for subscriptions or timers to work on - wait_for_work(timeout); - // Try again - any_exec = get_next_ready_executable(); - } - // At this point any_exec should be valid with either a valid subscription - // or a valid timer, or it should be a null shared_ptr - if (any_exec) { - // If it is valid, check to see if the group is mutually exclusive or - // not, then mark it accordingly - if (any_exec->callback_group && any_exec->callback_group->type() == \ - callback_group::CallbackGroupType::MutuallyExclusive) - { - // It should not have been taken otherwise - assert(any_exec->callback_group->can_be_taken_from().load()); - // Set to false to indicate something is being run from this group - any_exec->callback_group->can_be_taken_from().store(false); - } - } +AnyExecutable::SharedPtr +Executor::get_next_ready_executable() +{ + auto any_exec = memory_strategy_->instantiate_next_executable(); + // Check the timers to see if there are any that are ready, if so return + get_next_timer(any_exec); + if (any_exec->timer) { return any_exec; } + // Check the subscriptions to see if there are any that are ready + memory_strategy_->get_next_subscription(any_exec, weak_nodes_); + if (any_exec->subscription || any_exec->subscription_intra_process) { + return any_exec; + } + // Check the services to see if there are any that are ready + memory_strategy_->get_next_service(any_exec, weak_nodes_); + if (any_exec->service) { + return any_exec; + } + // Check the clients to see if there are any that are ready + memory_strategy_->get_next_client(any_exec, weak_nodes_); + if (any_exec->client) { + return any_exec; + } + // If there is no ready executable, return a null ptr + return nullptr; +} - /// Guard condition for signaling the rmw layer to wake up for special events. - rmw_guard_condition_t * interrupt_guard_condition_; - - /// The memory strategy: an interface for handling user-defined memory allocation strategies. - memory_strategy::MemoryStrategy::SharedPtr memory_strategy_; - -private: - RCLCPP_DISABLE_COPY(Executor); - - std::vector> weak_nodes_; - std::array guard_cond_handles_; -}; - -} // namespace executor -} // namespace rclcpp - -#endif // RCLCPP__EXECUTOR_HPP_ +AnyExecutable::SharedPtr +Executor::get_next_executable(std::chrono::nanoseconds timeout) +{ + // Check to see if there are any subscriptions or timers needing service + // TODO(wjwwood): improve run to run efficiency of this function + auto any_exec = get_next_ready_executable(); + // If there are none + if (!any_exec) { + // Wait for subscriptions or timers to work on + wait_for_work(timeout); + // Try again + any_exec = get_next_ready_executable(); + } + // At this point any_exec should be valid with either a valid subscription + // or a valid timer, or it should be a null shared_ptr + if (any_exec) { + // If it is valid, check to see if the group is mutually exclusive or + // not, then mark it accordingly + if (any_exec->callback_group && any_exec->callback_group->type() == \ + callback_group::CallbackGroupType::MutuallyExclusive) + { + // It should not have been taken otherwise + assert(any_exec->callback_group->can_be_taken_from().load()); + // Set to false to indicate something is being run from this group + any_exec->callback_group->can_be_taken_from().store(false); + } + } + return any_exec; +} diff --git a/rclcpp/src/rclcpp/executors.cpp b/rclcpp/src/rclcpp/executors.cpp index 045cc48..49d4197 100644 --- a/rclcpp/src/rclcpp/executors.cpp +++ b/rclcpp/src/rclcpp/executors.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,76 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_ -#define RCLCPP_RCLCPP_EXECUTORS_HPP_ +#include "rclcpp/executors.hpp" -#include -#include -#include -#include -#include - -namespace rclcpp +void +rclcpp::spin_some(node::Node::SharedPtr node_ptr) { -namespace executors -{ - -using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; -using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; - -/// Return codes to be used with spin_until_future_complete. -/** - * SUCCESS: The future is complete and can be accessed with "get" without blocking. - * INTERRUPTED: The future is not complete, spinning was interrupted by Ctrl-C or another error. - * TIMEOUT: Spinning timed out. - */ -enum FutureReturnCode {SUCCESS, INTERRUPTED, TIMEOUT}; - -/// Spin (blocking) until the future is complete, until the function times out (if applicable), -/// or until rclcpp is interrupted. -/** - * \param[in] executor The executor which will spin the node. - * \param[in] node_ptr The node to spin. - * \param[in] future The future to wait on. If SUCCESS, the future is safe to access after this function - * \param[in] timeout Optional timeout parameter, which gets passed to Executor::spin_node_once. - -1 is block forever, 0 is non-blocking. - If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return code. - * \return The return code, one of SUCCESS, INTERRUPTED, or TIMEOUT. - */ -template -FutureReturnCode -spin_node_until_future_complete( - rclcpp::executor::Executor & executor, rclcpp::node::Node::SharedPtr node_ptr, - std::shared_future & future, - std::chrono::duration timeout = std::chrono::duration(-1)) -{ - // TODO(wjwwood): does not work recursively right, can't call spin_node_until_future_complete - // inside a callback executed by an executor. - - // Check the future before entering the while loop. - // If the future is already complete, don't try to spin. - std::future_status status = future.wait_for(std::chrono::seconds(0)); - - auto start_time = std::chrono::system_clock::now(); - - while (status != std::future_status::ready && rclcpp::utilities::ok()) { - executor.spin_node_once(node_ptr, timeout); - if (timeout.count() >= 0) { - if (start_time + timeout < std::chrono::system_clock::now()) { - return TIMEOUT; - } - } - status = future.wait_for(std::chrono::seconds(0)); - } - - // If the future completed, and we weren't interrupted by ctrl-C, return the response - if (status == std::future_status::ready) { - return FutureReturnCode::SUCCESS; - } - return FutureReturnCode::INTERRUPTED; + rclcpp::executors::SingleThreadedExecutor exec; + exec.spin_node_some(node_ptr); } -} // namespace executors -} // namespace rclcpp - -#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ +void +rclcpp::spin(node::Node::SharedPtr node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node_ptr); + exec.spin(); +} diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index 82b1f84..185d8f7 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,95 +12,64 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ -#define RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ +#include "rclcpp/executors/multi_threaded_executor.hpp" -#include - -#include -#include -#include -#include +#include +#include #include -#include "rclcpp/executor.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" #include "rclcpp/utilities.hpp" -namespace rclcpp -{ -namespace executors -{ -namespace multi_threaded_executor -{ +using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; -class MultiThreadedExecutor : public executor::Executor +MultiThreadedExecutor::MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms) +: executor::Executor(ms) { -public: - RCLCPP_SMART_PTR_DEFINITIONS(MultiThreadedExecutor); + number_of_threads_ = std::thread::hardware_concurrency(); + if (number_of_threads_ == 0) { + number_of_threads_ = 1; + } +} - MultiThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : executor::Executor(ms) +MultiThreadedExecutor::~MultiThreadedExecutor() {} + +void +MultiThreadedExecutor::spin() +{ + std::vector threads; { - number_of_threads_ = std::thread::hardware_concurrency(); - if (number_of_threads_ == 0) { - number_of_threads_ = 1; + std::lock_guard wait_lock(wait_mutex_); + size_t thread_id = 1; + for (size_t i = number_of_threads_; i > 0; --i) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id++); + threads.emplace_back(func); } } + for (auto & thread : threads) { + thread.join(); + } +} - virtual ~MultiThreadedExecutor() {} +size_t +MultiThreadedExecutor::get_number_of_threads() +{ + return number_of_threads_; +} - void - spin() - { - std::vector threads; +void +MultiThreadedExecutor::run(size_t this_thread_number) +{ + thread_number_by_thread_id_[std::this_thread::get_id()] = this_thread_number; + while (rclcpp::utilities::ok()) { + executor::AnyExecutable::SharedPtr any_exec; { std::lock_guard wait_lock(wait_mutex_); - size_t thread_id_ = 1; // Use a _ suffix to avoid shadowing `rclcpp::thread_id` - for (size_t i = number_of_threads_; i > 0; --i) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - auto func = std::bind(&MultiThreadedExecutor::run, this, thread_id_++); - threads.emplace_back(func); + if (!rclcpp::utilities::ok()) { + return; } + any_exec = get_next_executable(); } - for (auto & thread : threads) { - thread.join(); - } + execute_any_executable(any_exec); } - - size_t - get_number_of_threads() - { - return number_of_threads_; - } - -private: - void run(size_t this_thread_id) - { - rclcpp::thread_id = this_thread_id; - while (rclcpp::utilities::ok()) { - executor::AnyExecutable::SharedPtr any_exec; - { - std::lock_guard wait_lock(wait_mutex_); - if (!rclcpp::utilities::ok()) { - return; - } - any_exec = get_next_executable(); - } - execute_any_executable(any_exec); - } - } - - RCLCPP_DISABLE_COPY(MultiThreadedExecutor); - - std::mutex wait_mutex_; - size_t number_of_threads_; -}; - -} // namespace multi_threaded_executor -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ +} diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index be87e19..66bddea 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,62 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ -#define RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ +#include -#include +using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; -#include -#include -#include -#include +SingleThreadedExecutor::SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms) +: executor::Executor(ms) {} -#include "rclcpp/executor.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/memory_strategies.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/utilities.hpp" -#include "rclcpp/rate.hpp" -namespace rclcpp +SingleThreadedExecutor::~SingleThreadedExecutor() {} + +void +SingleThreadedExecutor::spin() { -namespace executors -{ -namespace single_threaded_executor -{ - -/// Single-threaded executor implementation -// This is the default executor created by rclcpp::spin. -class SingleThreadedExecutor : public executor::Executor -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(SingleThreadedExecutor); - - /// Default constructor. See the default constructor for Executor. - SingleThreadedExecutor(memory_strategy::MemoryStrategy::SharedPtr ms = - memory_strategies::create_default_strategy()) - : executor::Executor(ms) {} - - /// Default destrcutor. - virtual ~SingleThreadedExecutor() {} - - /// Single-threaded implementation of spin. - // This function will block until work comes in, execute it, and keep blocking. - // It will only be interrupt by a CTRL-C (managed by the global signal handler). - void spin() - { - while (rclcpp::utilities::ok()) { - auto any_exec = get_next_executable(); - execute_any_executable(any_exec); - } + while (rclcpp::utilities::ok()) { + auto any_exec = get_next_executable(); + execute_any_executable(any_exec); } - -private: - RCLCPP_DISABLE_COPY(SingleThreadedExecutor); -}; - -} // namespace single_threaded_executor -} // namespace executors -} // namespace rclcpp - -#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_ +} diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp index d49d91c..508b7e8 100644 --- a/rclcpp/src/rclcpp/intra_process_manager.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -12,366 +12,64 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__INTRA_PROCESS_MANAGER_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_HPP_ +#include "rclcpp/intra_process_manager.hpp" -#include +using rclcpp::intra_process_manager::IntraProcessManager; -#include -#include -#include -#include -#include -#include -#include -#include +static std::atomic _next_unique_id {1}; -#include "rclcpp/allocator/allocator_deleter.hpp" -#include "rclcpp/intra_process_manager_state.hpp" -#include "rclcpp/mapped_ring_buffer.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/subscription.hpp" +IntraProcessManager::IntraProcessManager(IntraProcessManagerStateBase::SharedPtr state) +: state_(state) +{} -namespace rclcpp +IntraProcessManager::~IntraProcessManager() +{} + +uint64_t +IntraProcessManager::add_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription) { -namespace intra_process_manager + auto id = IntraProcessManager::get_next_unique_id(); + state_->add_subscription(id, subscription); + return id; +} + +void +IntraProcessManager::remove_subscription(uint64_t intra_process_subscription_id) { + state_->remove_subscription(intra_process_subscription_id); +} -/// This class facilitates intra process communication between nodes. -/* This class is used in the creation of publishers and subscriptions. - * A singleton instance of this class is owned by a rclcpp::Context and a - * rclcpp::Node can use an associated Context to get an instance of this class. - * Nodes which do not have a common Context will not exchange intra process - * messages because they will not share access to an instance of this class. - * - * When a Node creates a publisher or subscription, it will register them - * with this class. - * The node will also hook into the publisher's publish call - * in order to do intra process related work. - * - * When a publisher is created, it advertises on the topic the user provided, - * as well as a "shadowing" topic of type rcl_interfaces/IntraProcessMessage. - * For instance, if the user specified the topic '/namespace/chatter', then the - * corresponding intra process topic might be '/namespace/chatter__intra'. - * The publisher is also allocated an id which is unique among all publishers - * and subscriptions in this process. - * Additionally, when registered with this class a ring buffer is created and - * owned by this class as a temporary place to hold messages destined for intra - * process subscriptions. - * - * When a subscription is created, it subscribes to the topic provided by the - * user as well as to the corresponding intra process topic. - * It is also gets a unique id from the singleton instance of this class which - * is unique among publishers and subscriptions. - * - * When the user publishes a message, the message is stored by calling - * store_intra_process_message on this class. - * The instance of that message is uniquely identified by a publisher id and a - * message sequence number. - * The publisher id, message sequence pair is unique with in the process. - * At that point a list of the id's of intra process subscriptions which have - * been registered with the singleton instance of this class are stored with - * the message instance so that delivery is only made to those subscriptions. - * Then an instance of rcl_interfaces/IntraProcessMessage is published to the - * intra process topic which is specific to the topic specified by the user. - * - * When an instance of rcl_interfaces/IntraProcessMessage is received by a - * subscription, then it is handled by calling take_intra_process_message - * on a singleton of this class. - * The subscription passes a publisher id, message sequence pair which - * uniquely identifies the message instance it was suppose to receive as well - * as the subscriptions unique id. - * If the message is still being held by this class and the subscription's id - * is in the list of intended subscriptions then the message is returned. - * If either of those predicates are not satisfied then the message is not - * returned and the subscription does not call the users callback. - * - * Since the publisher builds a list of destined subscriptions on publish, and - * other requests are ignored, this class knows how many times a message - * instance should be requested. - * The final time a message is requested, the ownership is passed out of this - * class and passed to the final subscription, effectively freeing space in - * this class's internal storage. - * - * Since a topic is being used to ferry notifications about new intra process - * messages between publishers and subscriptions, it is possible for that - * notification to be lost. - * It is also possible that a subscription which was available when publish was - * called will no longer exist once the notification gets posted. - * In both cases this might result in a message instance getting requested - * fewer times than expected. - * This is why the internal storage of this class is a ring buffer. - * That way if a message is orphaned it will eventually be dropped from storage - * when a new message instance is stored and will not result in a memory leak. - * - * However, since the storage system is finite, this also means that a message - * instance might get displaced by an incoming message instance before all - * interested parties have called take_intra_process_message. - * Because of this the size of the internal storage should be carefully - * considered. - * - * /TODO(wjwwood): update to include information about handling latching. - * /TODO(wjwwood): consider thread safety of the class. - * - * This class is neither CopyConstructable nor CopyAssignable. - */ -class IntraProcessManager +void +IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) { -private: - RCLCPP_DISABLE_COPY(IntraProcessManager); + state_->remove_publisher(intra_process_publisher_id); +} -public: - RCLCPP_SMART_PTR_DEFINITIONS(IntraProcessManager); +bool +IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const +{ + return state_->matches_any_publishers(id); +} - explicit IntraProcessManager( - IntraProcessManagerStateBase::SharedPtr state = create_default_state() - ) - : state_(state) - { +uint64_t +IntraProcessManager::get_next_unique_id() +{ + auto next_id = _next_unique_id.fetch_add(1, std::memory_order_relaxed); + // Check for rollover (we started at 1). + if (0 == next_id) { + // This puts a technical limit on the number of times you can add a publisher or subscriber. + // But even if you could add (and remove) them at 1 kHz (very optimistic rate) + // it would still be a very long time before you could exhaust the pool of id's: + // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years + // So around 585 million years. Even at 1 GHz, it would take 585 years. + // I think it's safe to avoid trying to handle overflow. + // If we roll over then it's most likely a bug. + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::overflow_error( + "exhausted the unique id's for publishers and subscribers in this process " + "(congratulations your computer is either extremely fast or extremely old)"); + // *INDENT-ON* } - - /// Register a subscription with the manager, returns subscriptions unique id. - /* In addition to generating a unique intra process id for the subscription, - * this method also stores the topic name of the subscription. - * - * This method is normally called during the creation of a subscription, - * but after it creates the internal intra process rmw_subscription_t. - * - * This method will allocate memory. - * - * \param subscription the Subscription to register. - * \return an unsigned 64-bit integer which is the subscription's unique id. - */ - uint64_t - add_subscription(subscription::SubscriptionBase::SharedPtr subscription) - { - auto id = IntraProcessManager::get_next_unique_id(); - state_->add_subscription(id, subscription); - return id; - } - - /// Unregister a subscription using the subscription's unique id. - /* This method does not allocate memory. - * - * \param intra_process_subscription_id id of the subscription to remove. - */ - void - remove_subscription(uint64_t intra_process_subscription_id) - { - state_->remove_subscription(intra_process_subscription_id); - } - - /// Register a publisher with the manager, returns the publisher unique id. - /* In addition to generating and returning a unique id for the publisher, - * this method creates internal ring buffer storage for "in-flight" intra - * process messages which are stored when store_intra_process_message is - * called with this publisher's unique id. - * - * The buffer_size must be less than or equal to the max uint64_t value. - * If the buffer_size is 0 then a buffer size is calculated using the - * publisher's QoS settings. - * The default is to use the depth field of the publisher's QoS. - * TODO(wjwwood): Consider doing depth *= 1.2, round up, or similar. - * TODO(wjwwood): Consider what to do for keep all. - * - * This method is templated on the publisher's message type so that internal - * storage of the same type can be allocated. - * - * This method will allocate memory. - * - * \param publisher publisher to be registered with the manager. - * \param buffer_size if 0 (default) a size is calculated based on the QoS. - * \return an unsigned 64-bit integer which is the publisher's unique id. - */ - template - uint64_t - add_publisher(typename publisher::Publisher::SharedPtr publisher, - size_t buffer_size = 0) - { - auto id = IntraProcessManager::get_next_unique_id(); - size_t size = buffer_size > 0 ? buffer_size : publisher->get_queue_size(); - auto mrb = mapped_ring_buffer::MappedRingBuffer::MessageAlloc>::make_shared( - size, publisher->get_allocator()); - state_->add_publisher(id, publisher, mrb, size); - return id; - } - - /// Unregister a publisher using the publisher's unique id. - /* This method does not allocate memory. - * - * \param intra_process_publisher_id id of the publisher to remove. - */ - void - remove_publisher(uint64_t intra_process_publisher_id) - { - state_->remove_publisher(intra_process_publisher_id); - } - - /// Store a message in the manager, and return the message sequence number. - /* The given message is stored in internal storage using the given publisher - * id and the newly generated message sequence, which is also returned. - * The combination of publisher id and message sequence number can later - * be used with a subscription id to retrieve the message by calling - * take_intra_process_message. - * The number of times take_intra_process_message can be called with this - * unique pair of id's is determined by the number of subscriptions currently - * subscribed to the same topic and which share the same Context, i.e. once - * for each subscription which should receive the intra process message. - * - * The ownership of the incoming message is transfered to the internal - * storage in order to avoid copying the message data. - * Therefore, the message parameter will no longer contain the original - * message after calling this method. - * Instead it will either be a nullptr or it will contain the ownership of - * the message instance which was displaced. - * If the message parameter is not equal to nullptr after calling this method - * then a message was prematurely displaced, i.e. take_intra_process_message - * had not been called on it as many times as was expected. - * - * This method can throw an exception if the publisher id is not found or - * if the publisher shared_ptr given to add_publisher has gone out of scope. - * - * This method does allocate memory. - * - * \param intra_process_publisher_id the id of the publisher of this message. - * \param message the message that is being stored. - * \return the message sequence number. - */ - template, - typename Deleter = std::default_delete> - uint64_t - store_intra_process_message( - uint64_t intra_process_publisher_id, - std::unique_ptr & message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = typename mapped_ring_buffer::MappedRingBuffer; - uint64_t message_seq = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->get_publisher_info_for_id( - intra_process_publisher_id, message_seq); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - throw std::runtime_error("Typecast failed due to incorrect message type"); - } - - // Insert the message into the ring buffer using the message_seq to identify it. - bool did_replace = typed_buffer->push_and_replace(message_seq, message); - // TODO(wjwwood): do something when a message was displaced. log debug? - (void)did_replace; // Avoid unused variable warning. - - state_->store_intra_process_message(intra_process_publisher_id, message_seq); - - // Return the message sequence which is sent to the subscription. - return message_seq; - } - - /// Take an intra process message. - /* The intra_process_publisher_id and message_sequence_number parameters - * uniquely identify a message instance, which should be taken. - * - * The requesting_subscriptions_intra_process_id parameter is used to make - * sure the requesting subscription was intended to receive this message - * instance. - * This check is made because it could happen that the requester - * comes up after the publish event, so it still receives the notification of - * a new intra process message, but it wasn't registered with the manager at - * the time of publishing, causing it to take when it wasn't intended. - * This should be avioded unless latching-like behavior is involved. - * - * The message parameter is used to store the taken message. - * On the last expected call to this method, the ownership is transfered out - * of internal storage and into the message parameter. - * On all previous calls a copy of the internally stored message is made and - * the ownership of the copy is transfered to the message parameter. - * TODO(wjwwood): update this documentation when latching is supported. - * - * The message parameter can be set to nullptr if: - * - * - The publisher id is not found. - * - The message sequence is not found for the given publisher id. - * - The requesting subscription's id is not in the list of intended takers. - * - The requesting subscription's id has been used before with this message. - * - * This method may allocate memory to copy the stored message. - * - * \param intra_process_publisher_id the id of the message's publisher. - * \param message_sequence_number the sequence number of the message. - * \param requesting_subscriptions_intra_process_id the subscription's id. - * \param message the message typed unique_ptr used to return the message. - */ - template, - typename Deleter = std::default_delete> - void - take_intra_process_message( - uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - std::unique_ptr & message) - { - using MRBMessageAlloc = typename std::allocator_traits::template rebind_alloc; - using TypedMRB = mapped_ring_buffer::MappedRingBuffer; - message = nullptr; - - size_t target_subs_size = 0; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer = state_->take_intra_process_message( - intra_process_publisher_id, - message_sequence_number, - requesting_subscriptions_intra_process_id, - target_subs_size - ); - typename TypedMRB::SharedPtr typed_buffer = std::static_pointer_cast(buffer); - if (!typed_buffer) { - return; - } - // Return a copy or the unique_ptr (ownership) depending on how many subscriptions are left. - if (target_subs_size) { - // There are more subscriptions to serve, return a copy. - typed_buffer->get_copy_at_key(message_sequence_number, message); - } else { - // This is the last one to be returned, transfer ownership. - typed_buffer->pop_at_key(message_sequence_number, message); - } - } - - /// Return true if the given rmw_gid_t matches any stored Publishers. - bool - matches_any_publishers(const rmw_gid_t * id) const - { - return state_->matches_any_publishers(id); - } - -private: - static uint64_t get_next_unique_id() - { - auto next_id = next_unique_id_.fetch_add(1, std::memory_order_relaxed); - // Check for rollover (we started at 1). - if (0 == next_id) { - // This puts a technical limit on the number of times you can add a publisher or subscriber. - // But even if you could add (and remove) them at 1 kHz (very optimistic rate) - // it would still be a very long time before you could exhaust the pool of id's: - // 2^64 / 1000 times per sec / 60 sec / 60 min / 24 hours / 365 days = 584,942,417 years - // So around 585 million years. Even at 1 GHz, it would take 585 years. - // I think it's safe to avoid trying to handle overflow. - // If we roll over then it's most likely a bug. - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::overflow_error( - "exhausted the unique id's for publishers and subscribers in this process " - "(congratulations your computer is either extremely fast or extremely old)"); - // *INDENT-ON* - } - return next_id; - } - - static std::atomic next_unique_id_; - - IntraProcessManagerStateBase::SharedPtr state_; -}; - -std::atomic IntraProcessManager::next_unique_id_ {1}; - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_HPP_ + return next_id; +} diff --git a/rclcpp/src/rclcpp/intra_process_manager_state.cpp b/rclcpp/src/rclcpp/intra_process_manager_state.cpp index 8a4bc5e..113623f 100644 --- a/rclcpp/src/rclcpp/intra_process_manager_state.cpp +++ b/rclcpp/src/rclcpp/intra_process_manager_state.cpp @@ -12,259 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_ -#define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_ +#include "rclcpp/intra_process_manager_state.hpp" -#include -#include -#include - -#include -#include -#include -#include #include -#include -#include -#include -#include -#include -namespace rclcpp -{ -namespace intra_process_manager -{ - -class IntraProcessManagerStateBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(IntraProcessManagerStateBase); - - virtual void - add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) = 0; - - virtual void - remove_subscription(uint64_t intra_process_subscription_id) = 0; - - virtual void add_publisher(uint64_t id, - publisher::PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) = 0; - - virtual void - remove_publisher(uint64_t intra_process_publisher_id) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) = 0; - - virtual void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) = 0; - - virtual mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message(uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size) = 0; - - virtual bool - matches_any_publishers(const rmw_gid_t * id) const = 0; -}; - -template> -class IntraProcessManagerState : public IntraProcessManagerStateBase -{ -public: - void - add_subscription(uint64_t id, subscription::SubscriptionBase::SharedPtr subscription) - { - subscriptions_[id] = subscription; - subscription_ids_by_topic_[subscription->get_topic_name()].insert(id); - } - - void - remove_subscription(uint64_t intra_process_subscription_id) - { - subscriptions_.erase(intra_process_subscription_id); - for (auto & pair : subscription_ids_by_topic_) { - pair.second.erase(intra_process_subscription_id); - } - // Iterate over all publisher infos and all stored subscription id's and - // remove references to this subscription's id. - for (auto & publisher_pair : publishers_) { - for (auto & sub_pair : publisher_pair.second.target_subscriptions_by_message_sequence) { - sub_pair.second.erase(intra_process_subscription_id); - } - } - } - - void add_publisher(uint64_t id, - publisher::PublisherBase::WeakPtr publisher, - mapped_ring_buffer::MappedRingBufferBase::SharedPtr mrb, - size_t size) - { - publishers_[id].publisher = publisher; - // As long as the size of the ring buffer is less than the max sequence number, we're safe. - if (size > std::numeric_limits::max()) { - throw std::invalid_argument("the calculated buffer size is too large"); - } - publishers_[id].sequence_number.store(0); - - publishers_[id].buffer = mrb; - publishers_[id].target_subscriptions_by_message_sequence.reserve(size); - } - - void - remove_publisher(uint64_t intra_process_publisher_id) - { - publishers_.erase(intra_process_publisher_id); - } - - // return message_seq and mrb - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - get_publisher_info_for_id( - uint64_t intra_process_publisher_id, - uint64_t & message_seq) - { - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("store_intra_process_message called with invalid publisher id"); - } - PublisherInfo & info = it->second; - // Calculate the next message sequence number. - message_seq = info.sequence_number.fetch_add(1, std::memory_order_relaxed); - - return info.buffer; - } - - void - store_intra_process_message(uint64_t intra_process_publisher_id, uint64_t message_seq) - { - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - throw std::runtime_error("store_intra_process_message called with invalid publisher id"); - } - PublisherInfo & info = it->second; - auto publisher = info.publisher.lock(); - if (!publisher) { - throw std::runtime_error("publisher has unexpectedly gone out of scope"); - } - - // Figure out what subscriptions should receive the message. - auto & destined_subscriptions = subscription_ids_by_topic_[publisher->get_topic_name()]; - // Store the list for later comparison. - info.target_subscriptions_by_message_sequence[message_seq].clear(); - std::copy( - destined_subscriptions.begin(), destined_subscriptions.end(), - // Memory allocation occurs in info.target_subscriptions_by_message_sequence[message_seq] - std::inserter( - info.target_subscriptions_by_message_sequence[message_seq], - // This ends up only being a hint to std::set, could also be .begin(). - info.target_subscriptions_by_message_sequence[message_seq].end() - ) - ); - } - - mapped_ring_buffer::MappedRingBufferBase::SharedPtr - take_intra_process_message(uint64_t intra_process_publisher_id, - uint64_t message_sequence_number, - uint64_t requesting_subscriptions_intra_process_id, - size_t & size - ) - { - PublisherInfo * info; - { - auto it = publishers_.find(intra_process_publisher_id); - if (it == publishers_.end()) { - // Publisher is either invalid or no longer exists. - return 0; - } - info = &it->second; - } - // Figure out how many subscriptions are left. - AllocSet * target_subs; - { - auto it = info->target_subscriptions_by_message_sequence.find(message_sequence_number); - if (it == info->target_subscriptions_by_message_sequence.end()) { - // Message is no longer being stored by this publisher. - return 0; - } - target_subs = &it->second; - } - { - auto it = std::find( - target_subs->begin(), target_subs->end(), - requesting_subscriptions_intra_process_id); - if (it == target_subs->end()) { - // This publisher id/message seq pair was not intended for this subscription. - return 0; - } - target_subs->erase(it); - } - size = target_subs->size(); - return info->buffer; - } - - bool - matches_any_publishers(const rmw_gid_t * id) const - { - for (auto & publisher_pair : publishers_) { - auto publisher = publisher_pair.second.publisher.lock(); - if (!publisher) { - continue; - } - if (*publisher.get() == id) { - return true; - } - } - return false; - } - -private: - template - using RebindAlloc = typename std::allocator_traits::template rebind_alloc; - - using AllocSet = std::set, RebindAlloc>; - using SubscriptionMap = std::unordered_map, std::equal_to, - RebindAlloc>>; - using IDTopicMap = std::map, RebindAlloc>>; - - SubscriptionMap subscriptions_; - - IDTopicMap subscription_ids_by_topic_; - - struct PublisherInfo - { - RCLCPP_DISABLE_COPY(PublisherInfo); - - PublisherInfo() = default; - - publisher::PublisherBase::WeakPtr publisher; - std::atomic sequence_number; - mapped_ring_buffer::MappedRingBufferBase::SharedPtr buffer; - - using TargetSubscriptionsMap = std::unordered_map, std::equal_to, - RebindAlloc>>; - TargetSubscriptionsMap target_subscriptions_by_message_sequence; - }; - - using PublisherMap = std::unordered_map, std::equal_to, - RebindAlloc>>; - - PublisherMap publishers_; -}; - -static IntraProcessManagerStateBase::SharedPtr create_default_state() +rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr +rclcpp::intra_process_manager::create_default_state() { return std::make_shared>(); } - -} // namespace intra_process_manager -} // namespace rclcpp - -#endif // RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_ diff --git a/rclcpp/src/rclcpp/memory_strategies.cpp b/rclcpp/src/rclcpp/memory_strategies.cpp index f5a1a2a..754b8bb 100644 --- a/rclcpp/src/rclcpp/memory_strategies.cpp +++ b/rclcpp/src/rclcpp/memory_strategies.cpp @@ -12,25 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__MEMORY_STRATEGIES_HPP_ -#define RCLCPP__MEMORY_STRATEGIES_HPP_ +#include "rclcpp/memory_strategies.hpp" -#include -#include - -namespace rclcpp -{ -namespace memory_strategies -{ +#include "rclcpp/strategies/allocator_memory_strategy.hpp" using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; -static memory_strategy::MemoryStrategy::SharedPtr create_default_strategy() +rclcpp::memory_strategy::MemoryStrategy::SharedPtr +rclcpp::memory_strategies::create_default_strategy() { return std::make_shared>(); } - -} // namespace memory_strategies -} // namespace rclcpp - -#endif // RCLCPP__MEMORY_STRATEGIES_HPP_ diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp index 895ea9d..28c4bfd 100644 --- a/rclcpp/src/rclcpp/memory_strategy.cpp +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -12,244 +12,182 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__MEMORY_STRATEGY_HPP_ -#define RCLCPP__MEMORY_STRATEGY_HPP_ +#include "rclcpp/memory_strategy.hpp" -#include -#include +using rclcpp::memory_strategy::MemoryStrategy; -#include "rclcpp/any_executable.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/node.hpp" - -namespace rclcpp +rclcpp::subscription::SubscriptionBase::SharedPtr +MemoryStrategy::get_subscription_by_handle( + void * subscriber_handle, const WeakNodeVector & weak_nodes) { -namespace memory_strategy + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + for (auto & weak_subscription : group->get_subscription_ptrs()) { + auto subscription = weak_subscription.lock(); + if (subscription) { + if (subscription->get_subscription_handle()->data == subscriber_handle) { + return subscription; + } + if (subscription->get_intra_process_subscription_handle() && + subscription->get_intra_process_subscription_handle()->data == subscriber_handle) + { + return subscription; + } + } + } + } + } + return nullptr; +} + +rclcpp::service::ServiceBase::SharedPtr +MemoryStrategy::get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes) { + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { + continue; + } + for (auto & service : group->get_service_ptrs()) { + if (service->get_service_handle()->data == service_handle) { + return service; + } + } + } + } + return nullptr; +} -/// Delegate for handling memory allocations while the Executor is executing. -/** - * By default, the memory strategy dynamically allocates memory for structures that come in from - * the rmw implementation after the executor waits for work, based on the number of entities that - * come through. - */ -class MemoryStrategy +rclcpp::client::ClientBase::SharedPtr +MemoryStrategy::get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes) { -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(MemoryStrategy); - using WeakNodeVector = std::vector>; - - // return the new number of subscribers - virtual size_t fill_subscriber_handles(void ** & ptr) = 0; - - // return the new number of services - virtual size_t fill_service_handles(void ** & ptr) = 0; - - // return the new number of clients - virtual size_t fill_client_handles(void ** & ptr) = 0; - - virtual void clear_active_entities() = 0; - - virtual void clear_handles() = 0; - virtual void revalidate_handles() = 0; - virtual bool collect_entities(const WeakNodeVector & weak_nodes) = 0; - - /// Provide a newly initialized AnyExecutable object. - // \return Shared pointer to the fresh executable. - virtual executor::AnyExecutable::SharedPtr instantiate_next_executable() = 0; - - virtual void - get_next_subscription(executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; - - virtual void - get_next_service(executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; - - virtual void - get_next_client(executor::AnyExecutable::SharedPtr any_exec, - const WeakNodeVector & weak_nodes) = 0; - - static rclcpp::subscription::SubscriptionBase::SharedPtr - get_subscription_by_handle(void * subscriber_handle, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_subscription : group->get_subscription_ptrs()) { - auto subscription = weak_subscription.lock(); - if (subscription) { - if (subscription->get_subscription_handle()->data == subscriber_handle) { - return subscription; - } - if (subscription->get_intra_process_subscription_handle() && - subscription->get_intra_process_subscription_handle()->data == subscriber_handle) - { - return subscription; - } - } + for (auto & client : group->get_client_ptrs()) { + if (client->get_client_handle()->data == client_handle) { + return client; } } } + } + return nullptr; +} + +rclcpp::node::Node::SharedPtr +MemoryStrategy::get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, + const WeakNodeVector & weak_nodes) +{ + if (!group) { return nullptr; } + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto callback_group = weak_group.lock(); + if (callback_group == group) { + return node; + } + } + } + return nullptr; +} - static rclcpp::service::ServiceBase::SharedPtr - get_service_by_handle(void * service_handle, const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription, + const WeakNodeVector & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & service : group->get_service_ptrs()) { - if (service->get_service_handle()->data == service_handle) { - return service; - } + for (auto & weak_sub : group->get_subscription_ptrs()) { + auto sub = weak_sub.lock(); + if (sub == subscription) { + return group; } } } - return nullptr; } + return nullptr; +} - static rclcpp::client::ClientBase::SharedPtr - get_client_by_handle(void * client_handle, const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_service( + rclcpp::service::ServiceBase::SharedPtr service, + const WeakNodeVector & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; + } + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & client : group->get_client_ptrs()) { - if (client->get_client_handle()->data == client_handle) { - return client; - } + for (auto & serv : group->get_service_ptrs()) { + if (serv == service) { + return group; } } } - return nullptr; } + return nullptr; +} - static rclcpp::node::Node::SharedPtr - get_node_by_group(rclcpp::callback_group::CallbackGroup::SharedPtr group, - const WeakNodeVector & weak_nodes) - { - if (!group) { - return nullptr; +rclcpp::callback_group::CallbackGroup::SharedPtr +MemoryStrategy::get_group_by_client( + rclcpp::client::ClientBase::SharedPtr client, + const WeakNodeVector & weak_nodes) +{ + for (auto & weak_node : weak_nodes) { + auto node = weak_node.lock(); + if (!node) { + continue; } - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + if (!group) { continue; } - for (auto & weak_group : node->get_callback_groups()) { - auto callback_group = weak_group.lock(); - if (callback_group == group) { - return node; + for (auto & cli : group->get_client_ptrs()) { + if (cli == client) { + return group; } } } - return nullptr; } - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_subscription( - rclcpp::subscription::SubscriptionBase::SharedPtr subscription, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & weak_sub : group->get_subscription_ptrs()) { - auto sub = weak_sub.lock(); - if (sub == subscription) { - return group; - } - } - } - } - return nullptr; - } - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_service( - rclcpp::service::ServiceBase::SharedPtr service, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & serv : group->get_service_ptrs()) { - if (serv == service) { - return group; - } - } - } - } - return nullptr; - } - - static rclcpp::callback_group::CallbackGroup::SharedPtr - get_group_by_client(rclcpp::client::ClientBase::SharedPtr client, - const WeakNodeVector & weak_nodes) - { - for (auto & weak_node : weak_nodes) { - auto node = weak_node.lock(); - if (!node) { - continue; - } - for (auto & weak_group : node->get_callback_groups()) { - auto group = weak_group.lock(); - if (!group) { - continue; - } - for (auto & cli : group->get_client_ptrs()) { - if (cli == client) { - return group; - } - } - } - } - return nullptr; - } - - -}; - -} // namespace memory_strategy - -} // namespace rclcpp - -#endif // RCLCPP__MEMORY_STRATEGY_HPP_ + return nullptr; +} diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index b55d87f..2262b75 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,38 +12,53 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__NODE_IMPL_HPP_ -#define RCLCPP__NODE_IMPL_HPP_ - -#include -#include - #include -#include -#include #include #include -#include -#include -#include #include #include #include -#include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rosidl_generator_cpp/message_type_support.hpp" -#include "rosidl_generator_cpp/service_type_support.hpp" +#include "rclcpp/node.hpp" -#include "rclcpp/contexts/default_context.hpp" -#include "rclcpp/intra_process_manager.hpp" -#include "rclcpp/parameter.hpp" +// Specializations with references to unresolved symbols to delay evaluation until later. +// On Windows this is not necessary since it will be built against the rmw implementation directly. +#if !defined(WIN32) +namespace rosidl_generator_cpp +{ -#ifndef RCLCPP__NODE_HPP_ -#include "node.hpp" +template<> +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return rclcpp::type_support::get_parameter_event_msg_type_support(); +} + +template<> +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return rclcpp::type_support::get_set_parameters_result_msg_type_support(); +} + +template<> +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return rclcpp::type_support::get_parameter_descriptor_msg_type_support(); +} + +template<> +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return rclcpp::type_support::get_list_parameters_result_msg_type_support(); +} + +} // namespace rosidl_generator_cpp #endif -using namespace rclcpp; -using namespace node; +using rclcpp::node::Node; Node::Node(const std::string & node_name, bool use_intra_process_comms) : Node( @@ -106,6 +121,12 @@ Node::Node( "parameter_events", rmw_qos_profile_parameter_events); } +const std::string & +Node::get_name() const +{ + return name_; +} + rclcpp::callback_group::CallbackGroup::SharedPtr Node::create_callback_group( rclcpp::callback_group::CallbackGroupType group_type) @@ -117,95 +138,6 @@ Node::create_callback_group( return group; } -template -typename rclcpp::publisher::Publisher::SharedPtr -Node::create_publisher( - const std::string & topic_name, size_t qos_history_depth, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_publisher(topic_name, qos, allocator); -} - -template -typename publisher::Publisher::SharedPtr -Node::create_publisher( - const std::string & topic_name, const rmw_qos_profile_t & qos_profile, - std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - using rosidl_generator_cpp::get_message_type_support_handle; - auto type_support_handle = get_message_type_support_handle(); - rmw_publisher_t * publisher_handle = rmw_create_publisher( - node_handle_.get(), type_support_handle, topic_name.c_str(), qos_profile); - if (!publisher_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create publisher: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - - auto publisher = publisher::Publisher::make_shared( - node_handle_, publisher_handle, topic_name, qos_profile.depth, allocator); - - if (use_intra_process_comms_) { - rmw_publisher_t * intra_process_publisher_handle = rmw_create_publisher( - node_handle_.get(), ipm_ts_, (topic_name + "__intra").c_str(), qos_profile); - if (!intra_process_publisher_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create intra process publisher: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - - auto intra_process_manager = - context_->get_sub_context(); - uint64_t intra_process_publisher_id = - intra_process_manager->add_publisher(publisher); - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; - // *INDENT-OFF* - auto shared_publish_callback = - [weak_ipm](uint64_t publisher_id, void * msg, const std::type_info & type_info) -> uint64_t - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? - throw std::runtime_error( - "intra process publish called after destruction of intra process manager"); - } - if (!msg) { - throw std::runtime_error("cannot publisher msg which is a null pointer"); - } - auto & message_type_info = typeid(MessageT); - if (message_type_info != type_info) { - throw std::runtime_error( - std::string("published type '") + type_info.name() + - "' is incompatible from the publisher type '" + message_type_info.name() + "'"); - } - MessageT * typed_message_ptr = static_cast(msg); - using MessageDeleter = typename publisher::Publisher::MessageDeleter; - std::unique_ptr unique_msg(typed_message_ptr); - uint64_t message_seq = - ipm->store_intra_process_message(publisher_id, unique_msg); - return message_seq; - }; - // *INDENT-ON* - publisher->setup_intra_process( - intra_process_publisher_id, - shared_publish_callback, - intra_process_publisher_handle); - } - return publisher; -} - bool Node::group_in_node(callback_group::CallbackGroup::SharedPtr group) { @@ -219,138 +151,6 @@ Node::group_in_node(callback_group::CallbackGroup::SharedPtr group) return group_belongs_to_this_node; } -template -typename rclcpp::subscription::Subscription::SharedPtr -Node::create_subscription( - const std::string & topic_name, - CallbackT callback, - const rmw_qos_profile_t & qos_profile, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - typename std::shared_ptr allocator) -{ - if (!allocator) { - allocator = std::make_shared(); - } - - rclcpp::subscription::AnySubscriptionCallback any_subscription_callback(allocator); - any_subscription_callback.set(callback); - - using rosidl_generator_cpp::get_message_type_support_handle; - - if (!msg_mem_strat) { - msg_mem_strat = - rclcpp::message_memory_strategy::MessageMemoryStrategy::create_default(); - } - - auto type_support_handle = get_message_type_support_handle(); - rmw_subscription_t * subscriber_handle = rmw_create_subscription( - node_handle_.get(), type_support_handle, - topic_name.c_str(), qos_profile, ignore_local_publications); - if (!subscriber_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create subscription: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - using namespace rclcpp::subscription; - - auto sub = Subscription::make_shared( - node_handle_, - subscriber_handle, - topic_name, - ignore_local_publications, - any_subscription_callback, - msg_mem_strat); - auto sub_base_ptr = std::dynamic_pointer_cast(sub); - // Setup intra process. - if (use_intra_process_comms_) { - rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription( - node_handle_.get(), ipm_ts_, - (topic_name + "__intra").c_str(), qos_profile, false); - if (!subscriber_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create intra process subscription: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - auto intra_process_manager = - context_->get_sub_context(); - rclcpp::intra_process_manager::IntraProcessManager::WeakPtr weak_ipm = intra_process_manager; - uint64_t intra_process_subscription_id = - intra_process_manager->add_subscription(sub_base_ptr); - // *INDENT-OFF* - sub->setup_intra_process( - intra_process_subscription_id, - intra_process_subscriber_handle, - [weak_ipm]( - uint64_t publisher_id, - uint64_t message_sequence, - uint64_t subscription_id, - typename Subscription::MessageUniquePtr & message) - { - auto ipm = weak_ipm.lock(); - if (!ipm) { - // TODO(wjwwood): should this just return silently? Or maybe return with a logged warning? - throw std::runtime_error( - "intra process take called after destruction of intra process manager"); - } - ipm->take_intra_process_message( - publisher_id, message_sequence, subscription_id, message); - }, - [weak_ipm](const rmw_gid_t * sender_gid) -> bool { - auto ipm = weak_ipm.lock(); - if (!ipm) { - throw std::runtime_error( - "intra process publisher check called after destruction of intra process manager"); - } - return ipm->matches_any_publishers(sender_gid); - } - ); - // *INDENT-ON* - } - // Assign to a group. - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create subscription, group not in node."); - } - group->add_subscription(sub_base_ptr); - } else { - default_callback_group_->add_subscription(sub_base_ptr); - } - number_of_subscriptions_++; - return sub; -} - -template -typename rclcpp::subscription::Subscription::SharedPtr -Node::create_subscription( - const std::string & topic_name, - size_t qos_history_depth, - CallbackT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group, - bool ignore_local_publications, - typename rclcpp::message_memory_strategy::MessageMemoryStrategy::SharedPtr - msg_mem_strat, - std::shared_ptr allocator) -{ - rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.depth = qos_history_depth; - return this->create_subscription( - topic_name, - callback, - qos, - group, - ignore_local_publications, - msg_mem_strat, - allocator); -} - rclcpp::timer::WallTimer::SharedPtr Node::create_wall_timer( std::chrono::nanoseconds period, @@ -384,88 +184,6 @@ Node::create_wall_timer( // group); // } -template -typename client::Client::SharedPtr -Node::create_client( - const std::string & service_name, - rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - - rmw_client_t * client_handle = rmw_create_client( - this->node_handle_.get(), service_type_support_handle, service_name.c_str()); - if (!client_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create client: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - - using namespace rclcpp::client; - - auto cli = Client::make_shared( - node_handle_, - client_handle, - service_name); - - auto cli_base_ptr = std::dynamic_pointer_cast(cli); - if (group) { - if (!group_in_node(group)) { - // TODO(esteve): use custom exception - throw std::runtime_error("Cannot create client, group not in node."); - } - group->add_client(cli_base_ptr); - } else { - default_callback_group_->add_client(cli_base_ptr); - } - number_of_clients_++; - - return cli; -} - -template -typename rclcpp::service::Service::SharedPtr -Node::create_service( - const std::string & service_name, - FunctorT callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ - using rosidl_generator_cpp::get_service_type_support_handle; - auto service_type_support_handle = - get_service_type_support_handle(); - - rclcpp::service::AnyServiceCallback any_service_callback; - any_service_callback.set(callback); - - rmw_service_t * service_handle = rmw_create_service( - node_handle_.get(), service_type_support_handle, service_name.c_str()); - if (!service_handle) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("could not create service: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - - auto serv = service::Service::make_shared( - node_handle_, service_handle, service_name, any_service_callback); - auto serv_base_ptr = std::dynamic_pointer_cast(serv); - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create service, group not in node."); - } - group->add_service(serv_base_ptr); - } else { - default_callback_group_->add_service(serv_base_ptr); - } - number_of_services_++; - return serv; -} - std::vector Node::set_parameters( const std::vector & parameters) @@ -672,5 +390,3 @@ Node::get_callback_groups() const { return callback_groups_; } - -#endif // RCLCPP__NODE_IMPL_HPP_ diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp index f10be34..0e8fe5f 100644 --- a/rclcpp/src/rclcpp/parameter.cpp +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -12,262 +12,221 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_ -#define RCLCPP_RCLCPP_PARAMETER_HPP_ +#include "rclcpp/parameter.hpp" #include #include #include +#include -#include +using rclcpp::parameter::ParameterType; +using rclcpp::parameter::ParameterVariant; -#include -#include -#include - -namespace rclcpp +ParameterVariant::ParameterVariant() +: name_("") { + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} -namespace parameter +ParameterVariant::ParameterVariant(const std::string & name, const bool bool_value) +: name_(name) { + value_.bool_value = bool_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; +} -enum ParameterType +ParameterVariant::ParameterVariant(const std::string & name, const int int_value) +: name_(name) { - PARAMETER_NOT_SET=rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET, - PARAMETER_BOOL=rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, - PARAMETER_INTEGER=rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, - PARAMETER_DOUBLE=rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, - PARAMETER_STRING=rcl_interfaces::msg::ParameterType::PARAMETER_STRING, - PARAMETER_BYTES=rcl_interfaces::msg::ParameterType::PARAMETER_BYTES, -}; + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} -// Structure to store an arbitrary parameter with templated get/set methods -class ParameterVariant +ParameterVariant::ParameterVariant(const std::string & name, const int64_t int_value) +: name_(name) { -public: - ParameterVariant() - : name_("") - { - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; - } - explicit ParameterVariant(const std::string & name, const bool bool_value) - : name_(name) - { - value_.bool_value = bool_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - } - explicit ParameterVariant(const std::string & name, const int int_value) - : name_(name) - { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - } - explicit ParameterVariant(const std::string & name, const int64_t int_value) - : name_(name) - { - value_.integer_value = int_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - } - explicit ParameterVariant(const std::string & name, const float double_value) - : name_(name) - { - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - } - explicit ParameterVariant(const std::string & name, const double double_value) - : name_(name) - { - value_.double_value = double_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - } - explicit ParameterVariant(const std::string & name, const std::string & string_value) - : name_(name) - { - value_.string_value = string_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - } - explicit ParameterVariant(const std::string & name, const char * string_value) - : ParameterVariant(name, std::string(string_value)) {} - explicit ParameterVariant(const std::string & name, const std::vector & bytes_value) - : name_(name) - { - value_.bytes_value = bytes_value; - value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES; - } + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} - inline ParameterType get_type() const {return static_cast(value_.type); } +ParameterVariant::ParameterVariant(const std::string & name, const float double_value) +: name_(name) +{ + value_.double_value = double_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} - inline std::string get_type_name() const - { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return "bool"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return "integer"; - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return "double"; - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return "string"; - case rclcpp::parameter::ParameterType::PARAMETER_BYTES: - return "bytes"; - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } +ParameterVariant::ParameterVariant(const std::string & name, const double double_value) +: name_(name) +{ + value_.double_value = double_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; +} + +ParameterVariant::ParameterVariant(const std::string & name, const std::string & string_value) +: name_(name) +{ + value_.string_value = string_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; +} + +ParameterVariant::ParameterVariant(const std::string & name, const char * string_value) +: ParameterVariant(name, std::string(string_value)) +{} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & bytes_value) +: name_(name) +{ + value_.bytes_value = bytes_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTES; +} + +ParameterType +ParameterVariant::get_type() const +{ + return static_cast(value_.type); +} + +std::string +ParameterVariant::get_type_name() const +{ + switch (get_type()) { + case rclcpp::parameter::ParameterType::PARAMETER_BOOL: + return "bool"; + case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: + return "integer"; + case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: + return "double"; + case rclcpp::parameter::ParameterType::PARAMETER_STRING: + return "string"; + case rclcpp::parameter::ParameterType::PARAMETER_BYTES: + return "bytes"; + case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: + return "not set"; + default: + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + "Unexpected type from ParameterVariant: " + std::to_string(get_type())); + // *INDENT-ON* } +} - inline std::string get_name() const & {return name_; } +const std::string & +ParameterVariant::get_name() const +{ + return name_; +} - inline rcl_interfaces::msg::ParameterValue get_parameter_value() const - { - return value_; +rcl_interfaces::msg::ParameterValue +ParameterVariant::get_parameter_value() const +{ + return value_; +} + +int64_t +ParameterVariant::as_int() const +{ + return get_value(); +} + +double +ParameterVariant::as_double() const +{ + return get_value(); +} + +const std::string & +ParameterVariant::as_string() const +{ + return get_value(); +} + +bool +ParameterVariant::as_bool() const +{ + return get_value(); +} + +const std::vector & +ParameterVariant::as_bytes() const +{ + return get_value(); +} + +ParameterVariant +ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter) +{ + switch (parameter.value.type) { + case PARAMETER_BOOL: + return ParameterVariant(parameter.name, parameter.value.bool_value); + case PARAMETER_INTEGER: + return ParameterVariant(parameter.name, parameter.value.integer_value); + case PARAMETER_DOUBLE: + return ParameterVariant(parameter.name, parameter.value.double_value); + case PARAMETER_STRING: + return ParameterVariant(parameter.name, parameter.value.string_value); + case PARAMETER_BYTES: + return ParameterVariant(parameter.name, parameter.value.bytes_value); + case PARAMETER_NOT_SET: + throw std::runtime_error("Type from ParameterValue is not set"); + default: + // TODO(wjwwood): use custom exception + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type)); + // *INDENT-ON* } +} - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.integer_value; - } - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.double_value; - } - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.string_value; - } - template - typename std::enable_if::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bool_value; - } - template - typename std::enable_if &>::type - get_value() const - { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTES) { - // TODO: use custom exception - throw std::runtime_error("Invalid type"); - } - return value_.bytes_value; - } +rcl_interfaces::msg::Parameter +ParameterVariant::to_parameter() +{ + rcl_interfaces::msg::Parameter parameter; + parameter.name = name_; + parameter.value = value_; + return parameter; +} - int64_t as_int() const {return get_value(); } - - double as_double() const {return get_value(); } - - const std::string & as_string() const {return get_value(); } - - bool as_bool() const {return get_value(); } - - const std::vector & as_bytes() const - { - return get_value(); - } - - static ParameterVariant from_parameter(const rcl_interfaces::msg::Parameter & parameter) - { - switch (parameter.value.type) { - case PARAMETER_BOOL: - return ParameterVariant(parameter.name, parameter.value.bool_value); - case PARAMETER_INTEGER: - return ParameterVariant(parameter.name, parameter.value.integer_value); - case PARAMETER_DOUBLE: - return ParameterVariant(parameter.name, parameter.value.double_value); - case PARAMETER_STRING: - return ParameterVariant(parameter.name, parameter.value.string_value); - case PARAMETER_BYTES: - return ParameterVariant(parameter.name, parameter.value.bytes_value); - case PARAMETER_NOT_SET: - throw std::runtime_error("Type from ParameterValue is not set"); - default: - // TODO(wjwwood): use custom exception - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(parameter.value.type)); - // *INDENT-ON* - } - } - - rcl_interfaces::msg::Parameter to_parameter() - { - rcl_interfaces::msg::Parameter parameter; - parameter.name = name_; - parameter.value = value_; - return parameter; - } - - std::string value_to_string() const - { - switch (get_type()) { - case rclcpp::parameter::ParameterType::PARAMETER_BOOL: - return as_bool() ? "true" : "false"; - case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: - return std::to_string(as_int()); - case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: - return std::to_string(as_double()); - case rclcpp::parameter::ParameterType::PARAMETER_STRING: - return as_string(); - case rclcpp::parameter::ParameterType::PARAMETER_BYTES: - { - std::stringstream bytes; - bool first_byte = true; - bytes << "[" << std::hex; - for (auto & byte : as_bytes()) { - bytes << "0x" << byte; - if (!first_byte) { - bytes << ", "; - } else { - first_byte = false; - } +std::string +ParameterVariant::value_to_string() const +{ + switch (get_type()) { + case rclcpp::parameter::ParameterType::PARAMETER_BOOL: + return as_bool() ? "true" : "false"; + case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: + return std::to_string(as_int()); + case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE: + return std::to_string(as_double()); + case rclcpp::parameter::ParameterType::PARAMETER_STRING: + return as_string(); + case rclcpp::parameter::ParameterType::PARAMETER_BYTES: + { + std::stringstream bytes; + bool first_byte = true; + bytes << "[" << std::hex; + for (auto & byte : as_bytes()) { + bytes << "0x" << byte; + if (!first_byte) { + bytes << ", "; + } else { + first_byte = false; } - return bytes.str(); } - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; - default: - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - "Unexpected type from ParameterVariant: " + std::to_string(get_type())); - // *INDENT-ON* - } + return bytes.str(); + } + case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: + return "not set"; + default: + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + "Unexpected type from ParameterVariant: " + std::to_string(get_type())); + // *INDENT-ON* } +} -private: - std::string name_; - rcl_interfaces::msg::ParameterValue value_; -}; - - -/* Return a json encoded version of the parameter intended for a dict. */ -std::string _to_json_dict_entry(const ParameterVariant & param) +std::string +rclcpp::parameter::_to_json_dict_entry(const ParameterVariant & param) { std::stringstream ss; ss << "\"" << param.get_name() << "\": "; @@ -276,15 +235,22 @@ std::string _to_json_dict_entry(const ParameterVariant & param) return ss.str(); } - -} /* namespace parameter */ - -} /* namespace rclcpp */ - -namespace std +std::ostream & +rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv) { -/* Return a json encoded version of the parameter intended for a list. */ -inline std::string to_string(const rclcpp::parameter::ParameterVariant & param) + os << std::to_string(pv); + return os; +} + +std::ostream & +rclcpp::parameter::operator<<(std::ostream & os, const std::vector & parameters) +{ + os << std::to_string(parameters); + return os; +} + +std::string +std::to_string(const rclcpp::parameter::ParameterVariant & param) { std::stringstream ss; ss << "{\"name\": \"" << param.get_name() << "\", "; @@ -293,8 +259,8 @@ inline std::string to_string(const rclcpp::parameter::ParameterVariant & param) return ss.str(); } -/* Return a json encoded version of a vector of parameters, as a string*/ -inline std::string to_string(const std::vector & parameters) +std::string +std::to_string(const std::vector & parameters) { std::stringstream ss; ss << "{"; @@ -310,28 +276,3 @@ inline std::string to_string(const std::vector & parameters) -{ - os << std::to_string(parameters); - return os; -} - -} /* namespace parameter */ - -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */ diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 66314bd..9abe83e 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -12,348 +12,355 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ -#define RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ +#include "rclcpp/parameter_client.hpp" +#include #include +#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rclcpp +// Specializations with references to unresolved symbols to delay evaluation until later. +// On Windows this is not necessary since it will be built against the rmw implementation directly. +#if !defined(WIN32) +namespace rosidl_generator_cpp { -namespace parameter_client +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() { + return rclcpp::type_support::get_get_parameters_srv_type_support(); +} -class AsyncParametersClient +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() { + return rclcpp::type_support::get_get_parameter_types_srv_type_support(); +} -public: - RCLCPP_SMART_PTR_DEFINITIONS(AsyncParametersClient); - - AsyncParametersClient(const rclcpp::node::Node::SharedPtr node, - const std::string & remote_node_name = "") - : node_(node) - { - if (remote_node_name != "") { - remote_node_name_ = remote_node_name; - } else { - remote_node_name_ = node_->get_name(); - } - get_parameters_client_ = node_->create_client( - remote_node_name_ + "__get_parameters"); - get_parameter_types_client_ = node_->create_client( - remote_node_name_ + "__get_parameter_types"); - set_parameters_client_ = node_->create_client( - remote_node_name_ + "__set_parameters"); - list_parameters_client_ = node_->create_client( - remote_node_name_ + "__list_parameters"); - describe_parameters_client_ = node_->create_client( - remote_node_name_ + "__describe_parameters"); - } - - std::shared_future> - get_parameters( - const std::vector & names, - std::function>)> callback = nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->names = names; - - get_parameters_client_->async_send_request( - request, - [request, promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - std::vector parameter_variants; - auto & pvalues = cb_f.get()->values; - - for (auto & pvalue : pvalues) { - auto i = &pvalue - &pvalues[0]; - rcl_interfaces::msg::Parameter parameter; - parameter.name = request->names[i]; - parameter.value = pvalue; - parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter( - parameter)); - } - - promise_result->set_value(parameter_variants); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } - - std::shared_future> - get_parameter_types( - const std::vector & names, - std::function>)> callback = nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->names = names; - - get_parameter_types_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - std::vector types; - auto & pts = cb_f.get()->types; - for (auto & pt : pts) { - pts.push_back(static_cast(pt)); - } - promise_result->set_value(types); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } - - std::shared_future> - set_parameters( - const std::vector & parameters, - std::function>)> callback = - nullptr) - { - auto promise_result = - std::make_shared>>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - - std::transform(parameters.begin(), parameters.end(), std::back_inserter( - request->parameters), []( - rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); - - set_parameters_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->results); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } - - std::shared_future - set_parameters_atomically( - const std::vector & parameters, - std::function)> callback = - nullptr) - { - auto promise_result = - std::make_shared>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - - std::transform(parameters.begin(), parameters.end(), std::back_inserter( - request->parameters), []( - rclcpp::parameter::ParameterVariant p) {return p.to_parameter(); }); - - set_parameters_atomically_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->result); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } - - std::shared_future - list_parameters( - const std::vector & prefixes, - uint64_t depth, - std::function)> callback = - nullptr) - { - auto promise_result = - std::make_shared>(); - auto future_result = promise_result->get_future().share(); - - auto request = std::make_shared(); - request->prefixes = prefixes; - request->depth = depth; - - list_parameters_client_->async_send_request( - request, - [promise_result, future_result, &callback]( - rclcpp::client::Client::SharedFuture cb_f) { - promise_result->set_value(cb_f.get()->result); - if (callback != nullptr) { - callback(future_result); - } - } - ); - - return future_result; - } - - template - typename rclcpp::subscription::Subscription::SharedPtr - on_parameter_event(FunctorT callback) - { - return node_->create_subscription( - "parameter_events", callback, rmw_qos_profile_parameter_events); - } - -private: - const rclcpp::node::Node::SharedPtr node_; - rclcpp::client::Client::SharedPtr get_parameters_client_; - rclcpp::client::Client::SharedPtr - get_parameter_types_client_; - rclcpp::client::Client::SharedPtr set_parameters_client_; - rclcpp::client::Client::SharedPtr - set_parameters_atomically_client_; - rclcpp::client::Client::SharedPtr list_parameters_client_; - rclcpp::client::Client::SharedPtr - describe_parameters_client_; - std::string remote_node_name_; -}; - -class SyncParametersClient +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() { -public: - RCLCPP_SMART_PTR_DEFINITIONS(SyncParametersClient); + return rclcpp::type_support::get_set_parameters_srv_type_support(); +} - SyncParametersClient( - rclcpp::node::Node::SharedPtr node) - : node_(node) - { - executor_ = std::make_shared(); - async_parameters_client_ = std::make_shared(node); +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_list_parameters_srv_type_support(); +} + +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_describe_parameters_srv_type_support(); +} + +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_set_parameters_atomically_srv_type_support(); +} + +} // namespace rosidl_generator_cpp +#endif + +using rclcpp::parameter_client::AsyncParametersClient; +using rclcpp::parameter_client::SyncParametersClient; + +AsyncParametersClient::AsyncParametersClient( + const rclcpp::node::Node::SharedPtr node, + const std::string & remote_node_name) +: node_(node) +{ + if (remote_node_name != "") { + remote_node_name_ = remote_node_name; + } else { + remote_node_name_ = node_->get_name(); } + get_parameters_client_ = node_->create_client( + remote_node_name_ + "__get_parameters"); + get_parameter_types_client_ = node_->create_client( + remote_node_name_ + "__get_parameter_types"); + set_parameters_client_ = node_->create_client( + remote_node_name_ + "__set_parameters"); + list_parameters_client_ = node_->create_client( + remote_node_name_ + "__list_parameters"); + describe_parameters_client_ = node_->create_client( + remote_node_name_ + "__describe_parameters"); +} - SyncParametersClient( - rclcpp::executor::Executor::SharedPtr executor, - rclcpp::node::Node::SharedPtr node) - : executor_(executor), node_(node) - { - async_parameters_client_ = std::make_shared(node); - } +std::shared_future> +AsyncParametersClient::get_parameters( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); - std::vector - get_parameters(const std::vector & parameter_names) - { - auto f = async_parameters_client_->get_parameters(parameter_names); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) + auto request = std::make_shared(); + request->names = names; + + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + get_parameters_client_->async_send_request( + request, + [request, promise_result, future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { - return f.get(); + std::vector parameter_variants; + auto & pvalues = cb_f.get()->values; + + for (auto & pvalue : pvalues) { + auto i = &pvalue - &pvalues[0]; + rcl_interfaces::msg::Parameter parameter; + parameter.name = request->names[i]; + parameter.value = pvalue; + parameter_variants.push_back(rclcpp::parameter::ParameterVariant::from_parameter( + parameter)); + } + + promise_result->set_value(parameter_variants); + if (callback != nullptr) { + callback(future_result); + } } - // Return an empty vector if unsuccessful - return std::vector(); - } + ); + // *INDENT-ON* - std::vector - get_parameter_types(const std::vector & parameter_names) - { - auto f = async_parameters_client_->get_parameter_types(parameter_names); + return future_result; +} - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) +std::shared_future> +AsyncParametersClient::get_parameter_types( + const std::vector & names, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + request->names = names; + + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + get_parameter_types_client_->async_send_request( + request, + [promise_result, future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { - return f.get(); + std::vector types; + auto & pts = cb_f.get()->types; + for (auto & pt : pts) { + pts.push_back(static_cast(pt)); + } + promise_result->set_value(types); + if (callback != nullptr) { + callback(future_result); + } } - return std::vector(); - } + ); + // *INDENT-ON* - std::vector - set_parameters(const std::vector & parameters) - { - auto f = async_parameters_client_->set_parameters(parameters); + return future_result; +} - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) +std::shared_future> +AsyncParametersClient::set_parameters( + const std::vector & parameters, + std::function< + void(std::shared_future>) + > callback) +{ + auto promise_result = + std::make_shared>>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::parameter::ParameterVariant p) { + return p.to_parameter(); + } + ); + + set_parameters_client_->async_send_request( + request, + [promise_result, future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { - return f.get(); + promise_result->set_value(cb_f.get()->results); + if (callback != nullptr) { + callback(future_result); + } } - return std::vector(); - } + ); + // *INDENT-ON* - rcl_interfaces::msg::SetParametersResult - set_parameters_atomically(const std::vector & parameters) - { - auto f = async_parameters_client_->set_parameters_atomically(parameters); + return future_result; +} - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) +std::shared_future +AsyncParametersClient::set_parameters_atomically( + const std::vector & parameters, + std::function< + void(std::shared_future) + > callback) +{ + auto promise_result = + std::make_shared>(); + auto future_result = promise_result->get_future().share(); + + auto request = std::make_shared(); + + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + std::transform(parameters.begin(), parameters.end(), std::back_inserter(request->parameters), + [](rclcpp::parameter::ParameterVariant p) { + return p.to_parameter(); + } + ); + + set_parameters_atomically_client_->async_send_request( + request, + [promise_result, future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { - return f.get(); + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } } + ); + // *INDENT-ON* - throw std::runtime_error("Unable to get result of set parameters service call."); - } + return future_result; +} - rcl_interfaces::msg::ListParametersResult - list_parameters( - const std::vector & parameter_prefixes, - uint64_t depth) - { - auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); +std::shared_future +AsyncParametersClient::list_parameters( + const std::vector & prefixes, + uint64_t depth, + std::function< + void(std::shared_future) + > callback) +{ + auto promise_result = + std::make_shared>(); + auto future_result = promise_result->get_future().share(); - if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == - rclcpp::executors::FutureReturnCode::SUCCESS) + auto request = std::make_shared(); + request->prefixes = prefixes; + request->depth = depth; + + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + list_parameters_client_->async_send_request( + request, + [promise_result, future_result, &callback]( + rclcpp::client::Client::SharedFuture cb_f) { - return f.get(); + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } } + ); + // *INDENT-ON* - throw std::runtime_error("Unable to get result of list parameters service call."); - } + return future_result; +} - template - typename rclcpp::subscription::Subscription::SharedPtr - on_parameter_event(FunctorT callback) +SyncParametersClient::SyncParametersClient( + rclcpp::node::Node::SharedPtr node) +: node_(node) +{ + executor_ = std::make_shared(); + async_parameters_client_ = std::make_shared(node); +} + +SyncParametersClient::SyncParametersClient( + rclcpp::executor::Executor::SharedPtr executor, + rclcpp::node::Node::SharedPtr node) +: executor_(executor), node_(node) +{ + async_parameters_client_ = std::make_shared(node); +} + +std::vector +SyncParametersClient::get_parameters(const std::vector & parameter_names) +{ + auto f = async_parameters_client_->get_parameters(parameter_names); + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) { - return async_parameters_client_->on_parameter_event(callback); + return f.get(); + } + // Return an empty vector if unsuccessful + return std::vector(); +} + +std::vector +SyncParametersClient::get_parameter_types(const std::vector & parameter_names) +{ + auto f = async_parameters_client_->get_parameter_types(parameter_names); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +std::vector +SyncParametersClient::set_parameters( + const std::vector & parameters) +{ + auto f = async_parameters_client_->set_parameters(parameters); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } + return std::vector(); +} + +rcl_interfaces::msg::SetParametersResult +SyncParametersClient::set_parameters_atomically( + const std::vector & parameters) +{ + auto f = async_parameters_client_->set_parameters_atomically(parameters); + + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); } -private: - rclcpp::executor::Executor::SharedPtr executor_; - rclcpp::node::Node::SharedPtr node_; - AsyncParametersClient::SharedPtr async_parameters_client_; -}; + throw std::runtime_error("Unable to get result of set parameters service call."); +} -} /* namespace parameter_client */ +rcl_interfaces::msg::ListParametersResult +SyncParametersClient::list_parameters( + const std::vector & parameter_prefixes, + uint64_t depth) +{ + auto f = async_parameters_client_->list_parameters(parameter_prefixes, depth); -} /* namespace rclcpp */ + if (rclcpp::executors::spin_node_until_future_complete(*executor_, node_, f) == + rclcpp::executors::FutureReturnCode::SUCCESS) + { + return f.get(); + } -#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */ + throw std::runtime_error("Unable to get result of list parameters service call."); +} diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index ffc2983..e1efa70 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -12,166 +12,129 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ -#define RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ +#include "rclcpp/parameter_service.hpp" -#include +#include +#include -#include +using rclcpp::parameter_service::ParameterService; -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace rclcpp +ParameterService::ParameterService(const rclcpp::node::Node::SharedPtr node) +: node_(node) { + std::weak_ptr captured_node = node_; + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + get_parameters_service_ = node_->create_service( + node_->get_name() + "__get_parameters", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + auto values = node->get_parameters(request->names); + for (auto & pvariant : values) { + response->values.push_back(pvariant.get_parameter_value()); + } + } + ); -namespace parameter_service -{ + get_parameter_types_service_ = node_->create_service( + node_->get_name() + "__get_parameter_types", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + auto types = node->get_parameter_types(request->names); + std::transform(types.cbegin(), types.cend(), + std::back_inserter(response->types), [](const uint8_t & type) { + return static_cast(type); + }); + } + ); -class ParameterService -{ + set_parameters_service_ = node_->create_service( + node_->get_name() + "__set_parameters", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + std::vector pvariants; + for (auto & p : request->parameters) { + pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p)); + } + auto results = node->set_parameters(pvariants); + response->results = results; + } + ); -public: - RCLCPP_SMART_PTR_DEFINITIONS(ParameterService); + set_parameters_atomically_service_ = + node_->create_service( + node_->get_name() + "__set_parameters_atomically", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + std::vector pvariants; + std::transform(request->parameters.cbegin(), request->parameters.cend(), + std::back_inserter(pvariants), + [](const rcl_interfaces::msg::Parameter & p) { + return rclcpp::parameter::ParameterVariant:: + from_parameter(p); + }); + auto result = node->set_parameters_atomically(pvariants); + response->result = result; + } + ); - ParameterService(const rclcpp::node::Node::SharedPtr node) - : node_(node) - { - std::weak_ptr captured_node = node_; - get_parameters_service_ = node_->create_service( - node_->get_name() + "__get_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto values = node->get_parameters(request->names); - for (auto & pvariant : values) { - response->values.push_back(pvariant.get_parameter_value()); - } - } - ); + describe_parameters_service_ = node_->create_service( + node_->get_name() + "__describe_parameters", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + auto descriptors = node->describe_parameters(request->names); + response->descriptors = descriptors; + } + ); - get_parameter_types_service_ = node_->create_service( - node_->get_name() + "__get_parameter_types", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto types = node->get_parameter_types(request->names); - std::transform(types.cbegin(), types.cend(), - std::back_inserter(response->types), [](const uint8_t & type) { - return static_cast(type); - }); - } - ); - - set_parameters_service_ = node_->create_service( - node_->get_name() + "__set_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - std::vector pvariants; - for (auto & p : request->parameters) { - pvariants.push_back(rclcpp::parameter::ParameterVariant::from_parameter(p)); - } - auto results = node->set_parameters(pvariants); - response->results = results; - } - ); - - set_parameters_atomically_service_ = - node_->create_service( - node_->get_name() + "__set_parameters_atomically", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - std::vector pvariants; - std::transform(request->parameters.cbegin(), request->parameters.cend(), - std::back_inserter(pvariants), - [](const rcl_interfaces::msg::Parameter & p) { - return rclcpp::parameter::ParameterVariant:: - from_parameter(p); - }); - auto result = node->set_parameters_atomically(pvariants); - response->result = result; - } - ); - - describe_parameters_service_ = node_->create_service( - node_->get_name() + "__describe_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto descriptors = node->describe_parameters(request->names); - response->descriptors = descriptors; - } - ); - - list_parameters_service_ = node_->create_service( - node_->get_name() + "__list_parameters", [captured_node]( - const std::shared_ptr, - const std::shared_ptr request, - std::shared_ptr response) - { - auto node = captured_node.lock(); - if (!node) { - return; - } - auto result = node->list_parameters(request->prefixes, request->depth); - response->result = result; - } - ); - - - } - -private: - const rclcpp::node::Node::SharedPtr node_; - rclcpp::service::Service::SharedPtr get_parameters_service_; - rclcpp::service::Service::SharedPtr - get_parameter_types_service_; - rclcpp::service::Service::SharedPtr set_parameters_service_; - rclcpp::service::Service::SharedPtr - set_parameters_atomically_service_; - rclcpp::service::Service::SharedPtr - describe_parameters_service_; - rclcpp::service::Service::SharedPtr list_parameters_service_; -}; - -} /* namespace parameter_service */ - -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */ + list_parameters_service_ = node_->create_service( + node_->get_name() + "__list_parameters", + [captured_node]( + const std::shared_ptr, + const std::shared_ptr request, + std::shared_ptr response) + { + auto node = captured_node.lock(); + if (!node) { + return; + } + auto result = node->list_parameters(request->prefixes, request->depth); + response->result = result; + } + ); + // *INDENT-ON* +} diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index d0acec5..4ad9b87 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__PUBLISHER_HPP_ -#define RCLCPP__PUBLISHER_HPP_ +#include "rclcpp/publisher.hpp" #include #include @@ -27,328 +26,116 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" #include "rmw/impl/cpp/demangle.hpp" +#include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/allocator/allocator_deleter.hpp" #include "rclcpp/macros.hpp" -namespace rclcpp -{ +using rclcpp::publisher::PublisherBase; -// Forward declaration for friend statement -namespace node +PublisherBase::PublisherBase( + std::shared_ptr node_handle, + rmw_publisher_t * publisher_handle, + std::string topic, + size_t queue_size) +: node_handle_(node_handle), publisher_handle_(publisher_handle), + intra_process_publisher_handle_(nullptr), + topic_(topic), queue_size_(queue_size), + intra_process_publisher_id_(0), store_intra_process_message_(nullptr) { -class Node; -} // namespace node + // Life time of this object is tied to the publisher handle. + if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } +} -namespace publisher +PublisherBase::~PublisherBase() { - -class PublisherBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(PublisherBase); - /// Default constructor. - /** - * Typically, a publisher is not created through this method, but instead is created through a - * call to `Node::create_publisher`. - * \param[in] node_handle The corresponding rmw representation of the owner node. - * \param[in] publisher_handle The rmw publisher handle corresponding to this publisher. - * \param[in] topic The topic that this publisher publishes on. - * \param[in] queue_size The maximum number of unpublished messages to queue. - */ - PublisherBase( - std::shared_ptr node_handle, - rmw_publisher_t * publisher_handle, - std::string topic, - size_t queue_size) - : node_handle_(node_handle), publisher_handle_(publisher_handle), - intra_process_publisher_handle_(nullptr), - topic_(topic), queue_size_(queue_size), - intra_process_publisher_id_(0), store_intra_process_message_(nullptr) - { - // Life time of this object is tied to the publisher handle. - if (rmw_get_gid_for_publisher(publisher_handle_, &rmw_gid_) != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to get publisher gid: ") + rmw_get_error_string_safe()); - // *INDENT-ON* + if (intra_process_publisher_handle_) { + if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) { + fprintf( + stderr, + "Error in destruction of intra process rmw publisher handle: %s\n", + rmw_get_error_string_safe()); } } - - /// Default destructor. - virtual ~PublisherBase() - { - if (intra_process_publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), intra_process_publisher_handle_)) { - fprintf( - stderr, - "Error in destruction of intra process rmw publisher handle: %s\n", - rmw_get_error_string_safe()); - } - } - if (publisher_handle_) { - if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) { - fprintf( - stderr, - "Error in destruction of rmw publisher handle: %s\n", - rmw_get_error_string_safe()); - } + if (publisher_handle_) { + if (rmw_destroy_publisher(node_handle_.get(), publisher_handle_) != RMW_RET_OK) { + fprintf( + stderr, + "Error in destruction of rmw publisher handle: %s\n", + rmw_get_error_string_safe()); } } +} - /// Get the topic that this publisher publishes on. - // \return The topic name. - const std::string & - get_topic_name() const - { - return topic_; +const std::string & +PublisherBase::get_topic_name() const +{ + return topic_; +} + +size_t +PublisherBase::get_queue_size() const +{ + return queue_size_; +} + +const rmw_gid_t & +PublisherBase::get_gid() const +{ + return rmw_gid_; +} + +const rmw_gid_t & +PublisherBase::get_intra_process_gid() const +{ + return intra_process_rmw_gid_; +} + +bool +PublisherBase::operator==(const rmw_gid_t & gid) const +{ + return *this == &gid; +} + +bool +PublisherBase::operator==(const rmw_gid_t * gid) const +{ + bool result = false; + auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); + if (ret != RMW_RET_OK) { + throw std::runtime_error( + std::string("failed to compare gids: ") + rmw_get_error_string_safe()); } - - /// Get the queue size for this publisher. - // \return The queue size. - size_t - get_queue_size() const - { - return queue_size_; - } - - /// Get the global identifier for this publisher (used in rmw and by DDS). - // \return The gid. - const rmw_gid_t & - get_gid() const - { - return rmw_gid_; - } - - /// Get the global identifier for this publisher used by intra-process communication. - // \return The intra-process gid. - const rmw_gid_t & - get_intra_process_gid() const - { - return intra_process_rmw_gid_; - } - - /// Compare this publisher to a gid. - /** - * Note that this function calls the next function. - * \param[in] gid Reference to a gid. - * \return True if the publisher's gid matches the input. - */ - bool - operator==(const rmw_gid_t & gid) const - { - return *this == &gid; - } - - /// Compare this publisher to a pointer gid. - /** - * A wrapper for comparing this publisher's gid to the input using rmw_compare_gids_equal. - * \param[in] gid A pointer to a gid. - * \return True if this publisher's gid matches the input. - */ - bool - operator==(const rmw_gid_t * gid) const - { - bool result = false; - auto ret = rmw_compare_gids_equal(gid, &this->get_gid(), &result); + if (!result) { + ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); if (ret != RMW_RET_OK) { throw std::runtime_error( std::string("failed to compare gids: ") + rmw_get_error_string_safe()); } - if (!result) { - ret = rmw_compare_gids_equal(gid, &this->get_intra_process_gid(), &result); - if (ret != RMW_RET_OK) { - throw std::runtime_error( - std::string("failed to compare gids: ") + rmw_get_error_string_safe()); - } - } - return result; } + return result; +} - typedef std::function StoreMessageCallbackT; - -protected: - void - setup_intra_process( - uint64_t intra_process_publisher_id, - StoreMessageCallbackT callback, - rmw_publisher_t * intra_process_publisher_handle) - { - intra_process_publisher_id_ = intra_process_publisher_id; - store_intra_process_message_ = callback; - intra_process_publisher_handle_ = intra_process_publisher_handle; - // Life time of this object is tied to the publisher handle. - auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to create intra process publisher gid: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - } - - std::shared_ptr node_handle_; - - rmw_publisher_t * publisher_handle_; - rmw_publisher_t * intra_process_publisher_handle_; - - std::string topic_; - size_t queue_size_; - - uint64_t intra_process_publisher_id_; - StoreMessageCallbackT store_intra_process_message_; - - rmw_gid_t rmw_gid_; - rmw_gid_t intra_process_rmw_gid_; - - std::mutex intra_process_publish_mutex_; -}; - -/// A publisher publishes messages of any type to a topic. -template> -class Publisher : public PublisherBase +void +PublisherBase::setup_intra_process( + uint64_t intra_process_publisher_id, + StoreMessageCallbackT callback, + rmw_publisher_t * intra_process_publisher_handle) { - friend rclcpp::node::Node; - -public: - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; - - RCLCPP_SMART_PTR_DEFINITIONS(Publisher); - - Publisher( - std::shared_ptr node_handle, - rmw_publisher_t * publisher_handle, - std::string topic, - size_t queue_size, - std::shared_ptr allocator) - : PublisherBase(node_handle, publisher_handle, topic, queue_size) - { - message_allocator_ = std::make_shared(*allocator.get()); - allocator::set_allocator_for_deleter(&message_deleter_, message_allocator_.get()); + intra_process_publisher_id_ = intra_process_publisher_id; + store_intra_process_message_ = callback; + intra_process_publisher_handle_ = intra_process_publisher_handle; + // Life time of this object is tied to the publisher handle. + auto ret = rmw_get_gid_for_publisher(intra_process_publisher_handle_, &intra_process_rmw_gid_); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + throw std::runtime_error( + std::string("failed to create intra process publisher gid: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* } - - - /// Send a message to the topic for this publisher. - /** - * This function is templated on the input message type, MessageT. - * \param[in] msg A shared pointer to the message to send. - */ - void - publish(std::unique_ptr & msg) - { - this->do_inter_process_publish(msg.get()); - if (store_intra_process_message_) { - // Take the pointer from the unique_msg, release it and pass as a void * - // to the ipm. The ipm should then capture it again as a unique_ptr of - // the correct type. - // TODO(wjwwood): - // investigate how to transfer the custom deleter (if there is one) - // from the incoming unique_ptr through to the ipm's unique_ptr. - // See: http://stackoverflow.com/questions/11002641/dynamic-casting-for-unique-ptr - MessageT * msg_ptr = msg.get(); - msg.release(); - uint64_t message_seq; - { - std::lock_guard lock(intra_process_publish_mutex_); - message_seq = - store_intra_process_message_(intra_process_publisher_id_, msg_ptr, typeid(MessageT)); - } - rcl_interfaces::msg::IntraProcessMessage ipm; - ipm.publisher_id = intra_process_publisher_id_; - ipm.message_sequence = message_seq; - auto status = rmw_publish(intra_process_publisher_handle_, &ipm); - if (status != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to publish intra process message: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - } else { - // Always destroy the message, even if we don't consume it, for consistency. - msg.reset(); - } - } - - void - publish(const std::shared_ptr & msg) - { - // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg.get()); - } - // Otherwise we have to allocate memory in a unique_ptr and pass it along. - // TODO(wjwwood): - // The intra process manager should probably also be able to store - // shared_ptr's and do the "smart" thing based on other intra process - // subscriptions. For now call the other publish(). - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get()); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); - } - - void - publish(std::shared_ptr msg) - { - // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(msg.get()); - } - // Otherwise we have to allocate memory in a unique_ptr and pass it along. - // TODO(wjwwood): - // The intra process manager should probably also be able to store - // shared_ptr's and do the "smart" thing based on other intra process - // subscriptions. For now call the other publish(). - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, *msg.get()); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); - } - - void - publish(const MessageT & msg) - { - // Avoid allocating when not using intra process. - if (!store_intra_process_message_) { - // In this case we're not using intra process. - return this->do_inter_process_publish(&msg); - } - // Otherwise we have to allocate memory in a unique_ptr and pass it along. - auto ptr = MessageAllocTraits::allocate(*message_allocator_.get(), 1); - MessageAllocTraits::construct(*message_allocator_.get(), ptr, msg); - MessageUniquePtr unique_msg(ptr, message_deleter_); - return this->publish(unique_msg); - } - - std::shared_ptr get_allocator() const - { - return message_allocator_; - } - -protected: - void - do_inter_process_publish(const MessageT * msg) - { - auto status = rmw_publish(publisher_handle_, msg); - if (status != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to publish message: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - } - - std::shared_ptr message_allocator_; - - MessageDeleter message_deleter_; -}; - -} // namespace publisher -} // namespace rclcpp - -#endif // RCLCPP__PUBLISHER_HPP_ +} diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 2bfbb09..f2f588c 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_SERVICE_HPP_ -#define RCLCPP_RCLCPP_SERVICE_HPP_ +#include "rclcpp/service.hpp" #include #include @@ -21,138 +20,40 @@ #include #include -#include -#include +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/macros.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" -#include -#include +using rclcpp::service::ServiceBase; -namespace rclcpp +ServiceBase::ServiceBase( + std::shared_ptr node_handle, + rmw_service_t * service_handle, + const std::string service_name) +: node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) +{} + +ServiceBase::~ServiceBase() { - -namespace service -{ - -class ServiceBase -{ - -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(ServiceBase); - - ServiceBase( - std::shared_ptr node_handle, - rmw_service_t * service_handle, - const std::string service_name) - : node_handle_(node_handle), service_handle_(service_handle), service_name_(service_name) - {} - - virtual ~ServiceBase() - { - if (service_handle_) { - if (rmw_destroy_service(service_handle_) != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw service_handle_ handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } + if (service_handle_) { + if (rmw_destroy_service(service_handle_) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rmw service_handle_ handle: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); } } +} - std::string get_service_name() - { - return this->service_name_; - } - - const rmw_service_t * get_service_handle() - { - return this->service_handle_; - } - - virtual std::shared_ptr create_request() = 0; - virtual std::shared_ptr create_request_header() = 0; - virtual void handle_request( - std::shared_ptr request_header, - std::shared_ptr request) = 0; - -private: - RCLCPP_DISABLE_COPY(ServiceBase); - - std::shared_ptr node_handle_; - - rmw_service_t * service_handle_; - std::string service_name_; - -}; - -using namespace any_service_callback; - -template -class Service : public ServiceBase +std::string +ServiceBase::get_service_name() { -public: - using CallbackType = std::function< - void( - const std::shared_ptr, - std::shared_ptr)>; + return this->service_name_; +} - using CallbackWithHeaderType = std::function< - void( - const std::shared_ptr, - const std::shared_ptr, - std::shared_ptr)>; - RCLCPP_SMART_PTR_DEFINITIONS(Service); - - Service( - std::shared_ptr node_handle, - rmw_service_t * service_handle, - const std::string & service_name, - AnyServiceCallback any_callback) - : ServiceBase(node_handle, service_handle, service_name), any_callback_(any_callback) - {} - - Service() = delete; - - std::shared_ptr create_request() - { - return std::shared_ptr(new typename ServiceT::Request()); - } - - std::shared_ptr create_request_header() - { - // TODO(wjwwood): This should probably use rmw_request_id's allocator. - // (since it is a C type) - return std::shared_ptr(new rmw_request_id_t); - } - - void handle_request(std::shared_ptr request_header, std::shared_ptr request) - { - auto typed_request = std::static_pointer_cast(request); - auto typed_request_header = std::static_pointer_cast(request_header); - auto response = std::shared_ptr(new typename ServiceT::Response); - any_callback_.dispatch(typed_request_header, typed_request, response); - send_response(typed_request_header, response); - } - - void send_response( - std::shared_ptr req_id, - std::shared_ptr response) - { - rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get()); - if (status != RMW_RET_OK) { - // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) - throw std::runtime_error( - std::string("failed to send response: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - } - -private: - RCLCPP_DISABLE_COPY(Service); - - AnyServiceCallback any_callback_; -}; - -} /* namespace service */ -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */ +const rmw_service_t * +ServiceBase::get_service_handle() +{ + return this->service_handle_; +} diff --git a/rclcpp/src/rclcpp/subscription.cpp b/rclcpp/src/rclcpp/subscription.cpp index fb9c952..caf6578 100644 --- a/rclcpp/src/rclcpp/subscription.cpp +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,273 +12,67 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__SUBSCRIPTION_HPP_ -#define RCLCPP__SUBSCRIPTION_HPP_ +#include "rclcpp/subscription.hpp" -#include -#include - -#include -#include -#include -#include +#include #include -#include "rcl_interfaces/msg/intra_process_message.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" -#include "rclcpp/macros.hpp" -#include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/any_subscription_callback.hpp" -namespace rclcpp +using rclcpp::subscription::SubscriptionBase; + +SubscriptionBase::SubscriptionBase( + std::shared_ptr node_handle, + rmw_subscription_t * subscription_handle, + const std::string & topic_name, + bool ignore_local_publications) +: intra_process_subscription_handle_(nullptr), + node_handle_(node_handle), + subscription_handle_(subscription_handle), + topic_name_(topic_name), + ignore_local_publications_(ignore_local_publications) { + // To avoid unused private member warnings. + (void)ignore_local_publications_; +} -namespace node +SubscriptionBase::~SubscriptionBase() { -class Node; -} // namespace node - -namespace subscription -{ - -/// Virtual base class for subscriptions. This pattern allows us to iterate over different template -/// specializations of Subscription, among other things. -class SubscriptionBase -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(SubscriptionBase); - - /// Default constructor. - /** - * \param[in] node_handle The rmw representation of the node that owns this subscription. - * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] ignore_local_publications True to ignore local publications (unused). - */ - SubscriptionBase( - std::shared_ptr node_handle, - rmw_subscription_t * subscription_handle, - const std::string & topic_name, - bool ignore_local_publications) - : intra_process_subscription_handle_(nullptr), - node_handle_(node_handle), - subscription_handle_(subscription_handle), - topic_name_(topic_name), - ignore_local_publications_(ignore_local_publications) - { - // To avoid unused private member warnings. - (void)ignore_local_publications_; - } - - /// Default destructor. - virtual ~SubscriptionBase() - { - if (subscription_handle_) { - if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw subscription handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } - } - if (intra_process_subscription_handle_) { - auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_); - if (ret != RMW_RET_OK) { - std::stringstream ss; - ss << "Error in destruction of rmw intra process subscription handle: " << - rmw_get_error_string_safe() << '\n'; - (std::cerr << ss.str()).flush(); - } + if (subscription_handle_) { + if (rmw_destroy_subscription(node_handle_.get(), subscription_handle_) != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rmw subscription handle: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); } } - - /// Get the topic that this subscription is subscribed on. - const std::string & get_topic_name() const - { - return this->topic_name_; + if (intra_process_subscription_handle_) { + auto ret = rmw_destroy_subscription(node_handle_.get(), intra_process_subscription_handle_); + if (ret != RMW_RET_OK) { + std::stringstream ss; + ss << "Error in destruction of rmw intra process subscription handle: " << + rmw_get_error_string_safe() << '\n'; + (std::cerr << ss.str()).flush(); + } } +} - const rmw_subscription_t * get_subscription_handle() const - { - return subscription_handle_; - } - - const rmw_subscription_t * get_intra_process_subscription_handle() const - { - return intra_process_subscription_handle_; - } - - /// Borrow a new message. - // \return Shared pointer to the fresh message. - virtual std::shared_ptr create_message() = 0; - /// Check if we need to handle the message, and execute the callback if we do. - /** - * \param[in] message Shared pointer to the message to handle. - * \param[in] message_info Metadata associated with this message. - */ - virtual void handle_message( - std::shared_ptr & message, - const rmw_message_info_t & message_info) = 0; - - /// Return the message borrowed in create_message. - // \param[in] Shared pointer to the returned message. - virtual void return_message(std::shared_ptr & message) = 0; - virtual void handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) = 0; - -protected: - rmw_subscription_t * intra_process_subscription_handle_; - -private: - RCLCPP_DISABLE_COPY(SubscriptionBase); - - std::shared_ptr node_handle_; - - rmw_subscription_t * subscription_handle_; - - std::string topic_name_; - bool ignore_local_publications_; -}; - -using namespace any_subscription_callback; - -/// Subscription implementation, templated on the type of message this subscription receives. -template> -class Subscription : public SubscriptionBase +const std::string & +SubscriptionBase::get_topic_name() const { - friend class rclcpp::node::Node; + return this->topic_name_; +} -public: - using MessageAllocTraits = allocator::AllocRebind; - using MessageAlloc = typename MessageAllocTraits::allocator_type; - using MessageDeleter = allocator::Deleter; - using MessageUniquePtr = std::unique_ptr; +const rmw_subscription_t * +SubscriptionBase::get_subscription_handle() const +{ + return subscription_handle_; +} - RCLCPP_SMART_PTR_DEFINITIONS(Subscription); - - /// Default constructor. - /** - * The constructor for a subscription is almost never called directly. Instead, subscriptions - * should be instantiated through Node::create_subscription. - * \param[in] node_handle rmw representation of the node that owns this subscription. - * \param[in] topic_name Name of the topic to subscribe to. - * \param[in] ignore_local_publications True to ignore local publications (unused). - * \param[in] callback User-defined callback to call when a message is received. - * \param[in] memory_strategy The memory strategy to be used for managing message memory. - */ - Subscription( - std::shared_ptr node_handle, - rmw_subscription_t * subscription_handle, - const std::string & topic_name, - bool ignore_local_publications, - AnySubscriptionCallback callback, - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr memory_strategy = - message_memory_strategy::MessageMemoryStrategy::create_default()) - : SubscriptionBase(node_handle, subscription_handle, topic_name, ignore_local_publications), - any_callback_(callback), - message_memory_strategy_(memory_strategy), - get_intra_process_message_callback_(nullptr), - matches_any_intra_process_publishers_(nullptr) - { - } - - /// Support dynamically setting the message memory strategy. - /** - * Behavior may be undefined if called while the subscription could be executing. - * \param[in] message_memory_strategy Shared pointer to the memory strategy to set. - */ - void set_message_memory_strategy( - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr message_memory_strategy) - { - message_memory_strategy_ = message_memory_strategy; - } - std::shared_ptr create_message() - { - /* The default message memory strategy provides a dynamically allocated message on each call to - * create_message, though alternative memory strategies that re-use a preallocated message may be - * used (see rclcpp/strategies/message_pool_memory_strategy.hpp). - */ - return message_memory_strategy_->borrow_message(); - } - - void handle_message(std::shared_ptr & message, const rmw_message_info_t & message_info) - { - if (matches_any_intra_process_publishers_) { - if (matches_any_intra_process_publishers_(&message_info.publisher_gid)) { - // In this case, the message will be delivered via intra process and - // we should ignore this copy of the message. - return; - } - } - auto typed_message = std::static_pointer_cast(message); - any_callback_.dispatch(typed_message, message_info); - } - - void return_message(std::shared_ptr & message) - { - auto typed_message = std::static_pointer_cast(message); - message_memory_strategy_->return_message(typed_message); - } - - void handle_intra_process_message( - rcl_interfaces::msg::IntraProcessMessage & ipm, - const rmw_message_info_t & message_info) - { - if (!get_intra_process_message_callback_) { - // throw std::runtime_error( - // "handle_intra_process_message called before setup_intra_process"); - // TODO(wjwwood): for now, this could mean that intra process was just not enabled. - // However, this can only really happen if this node has it disabled, but the other doesn't. - return; - } - MessageUniquePtr msg; - get_intra_process_message_callback_( - ipm.publisher_id, - ipm.message_sequence, - intra_process_subscription_id_, - msg); - if (!msg) { - // This either occurred because the publisher no longer exists or the - // message requested is no longer being stored. - // TODO(wjwwood): should we notify someone of this? log error, log warning? - return; - } - any_callback_.dispatch_intra_process(msg, message_info); - } - -private: - typedef - std::function< - void (uint64_t, uint64_t, uint64_t, MessageUniquePtr &) - > GetMessageCallbackType; - typedef std::function MatchesAnyPublishersCallbackType; - - void setup_intra_process( - uint64_t intra_process_subscription_id, - rmw_subscription_t * intra_process_subscription, - GetMessageCallbackType get_message_callback, - MatchesAnyPublishersCallbackType matches_any_publisher_callback) - { - intra_process_subscription_id_ = intra_process_subscription_id; - intra_process_subscription_handle_ = intra_process_subscription; - get_intra_process_message_callback_ = get_message_callback; - matches_any_intra_process_publishers_ = matches_any_publisher_callback; - } - - RCLCPP_DISABLE_COPY(Subscription); - - AnySubscriptionCallback any_callback_; - typename message_memory_strategy::MessageMemoryStrategy::SharedPtr - message_memory_strategy_; - - GetMessageCallbackType get_intra_process_message_callback_; - MatchesAnyPublishersCallbackType matches_any_intra_process_publishers_; - uint64_t intra_process_subscription_id_; -}; - -} // namespace subscription -} // namespace rclcpp - -#endif // RCLCPP__SUBSCRIPTION_HPP_ +const rmw_subscription_t * +SubscriptionBase::get_intra_process_subscription_handle() const +{ + return intra_process_subscription_handle_; +} diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 9a69cb8..2ee84ee 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,166 +12,36 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_TIMER_HPP_ -#define RCLCPP_RCLCPP_TIMER_HPP_ +#include "rclcpp/timer.hpp" #include -#include -#include -#include -#include -#include -#include +using rclcpp::timer::CallbackType; +using rclcpp::timer::TimerBase; -#include -#include -#include +TimerBase::TimerBase(std::chrono::nanoseconds period, CallbackType callback) +: period_(period), + callback_(callback), + canceled_(false) +{} -namespace rclcpp +TimerBase::~TimerBase() +{} + +void +TimerBase::cancel() { + this->canceled_ = true; +} -namespace timer +void +TimerBase::execute_callback() const { + callback_(); +} -using CallbackType = std::function; - -class TimerBase +const CallbackType & +TimerBase::get_callback() const { - -public: - RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(TimerBase); - - TimerBase(std::chrono::nanoseconds period, CallbackType callback) - : period_(period), - callback_(callback), - canceled_(false) - { - } - - virtual ~TimerBase() - { - } - - void - cancel() - { - this->canceled_ = true; - } - - void execute_callback() const - { - callback_(); - } - - const CallbackType & get_callback() const - { - return callback_; - } - - /// Check how long the timer has until its next scheduled callback. - // \return A std::chrono::duration representing the relative time until the next callback. - virtual std::chrono::nanoseconds - time_until_trigger() = 0; - - /// Is the clock steady (i.e. is the time between ticks constant?) - // \return True if the clock used by this timer is steady. - virtual bool is_steady() = 0; - - /// Check if the timer needs to trigger the callback. - /** - * This function expects its caller to immediately trigger the callback after this function, - * since it maintains the last time the callback was triggered. - * \return True if the timer needs to trigger. - */ - virtual bool check_and_trigger() = 0; - -protected: - std::chrono::nanoseconds period_; - CallbackType callback_; - - bool canceled_; - -}; - -/// Generic timer templated on the clock type. Periodically executes a user-specified callback. -template -class GenericTimer : public TimerBase -{ - -public: - RCLCPP_SMART_PTR_DEFINITIONS(GenericTimer); - - /// Default constructor. - /** - * \param[in] period The interval at which the timer fires. - * \param[in] callback User-specified callback function. - */ - GenericTimer(std::chrono::nanoseconds period, CallbackType callback) - : TimerBase(period, callback), loop_rate_(period) - { - /* Subtracting the loop rate period ensures that the callback gets triggered - on the first call to check_and_trigger. */ - last_triggered_time_ = Clock::now() - period; - } - - /// Default destructor. - virtual ~GenericTimer() - { - // Stop the timer from running. - cancel(); - } - - bool - check_and_trigger() - { - if (canceled_) { - return false; - } - if (Clock::now() < last_triggered_time_) { - return false; - } - if (std::chrono::duration_cast(Clock::now() - last_triggered_time_) >= - loop_rate_.period()) - { - last_triggered_time_ = Clock::now(); - return true; - } - return false; - } - - std::chrono::nanoseconds - time_until_trigger() - { - std::chrono::nanoseconds time_until_trigger; - // Calculate the time between the next trigger and the current time - if (last_triggered_time_ + loop_rate_.period() < Clock::now()) { - // time is overdue, need to trigger immediately - time_until_trigger = std::chrono::nanoseconds::zero(); - } else { - time_until_trigger = std::chrono::duration_cast( - last_triggered_time_ - Clock::now()) + loop_rate_.period(); - } - return time_until_trigger; - } - - virtual bool - is_steady() - { - return Clock::is_steady; - } - -private: - RCLCPP_DISABLE_COPY(GenericTimer); - - rclcpp::rate::GenericRate loop_rate_; - std::chrono::time_point last_triggered_time_; - -}; - -using WallTimer = GenericTimer; - -} /* namespace timer */ -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */ + return callback_; +} diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 520818e..4ae1b55 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -1,4 +1,4 @@ -// Copyright 2014 Open Source Robotics Foundation, Inc. +// Copyright 2015 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. @@ -12,54 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ -#define RCLCPP_RCLCPP_UTILITIES_HPP_ +#include "rclcpp/utilities.hpp" -// TODO(wjwwood): remove -#include - -#include -#include +#include #include #include +#include #include #include -#include -#include +#include -#include -#include -#include +#include "rmw/error_handling.h" +#include "rmw/rmw.h" // Determine if sigaction is available #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE #define HAS_SIGACTION #endif -namespace -{ /// Represent the status of the global interrupt signal. -volatile sig_atomic_t g_signal_status = 0; +static volatile sig_atomic_t g_signal_status = 0; /// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. -rmw_guard_condition_t * g_sigint_guard_cond_handle = \ - rmw_create_guard_condition(); +static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition(); /// Condition variable for timed sleep (see sleep_for). -std::condition_variable g_interrupt_condition_variable; -std::atomic g_is_interrupted(false); +static std::condition_variable g_interrupt_condition_variable; +static std::atomic g_is_interrupted(false); /// Mutex for protecting the global condition variable. -std::mutex g_interrupt_mutex; +static std::mutex g_interrupt_mutex; #ifdef HAS_SIGACTION -struct sigaction old_action; +static struct sigaction old_action; #else -void (* old_signal_handler)(int) = 0; +static void (* old_signal_handler)(int) = 0; #endif -/// Handle the interrupt signal. -/** When the interrupt signal fires, the signal handler notifies the condition variable to wake up - * and triggers the interrupt guard condition, so that all global threads managed by rclcpp - * are interrupted. - */ void #ifdef HAS_SIGACTION signal_handler(int signal_value, siginfo_t * siginfo, void * context) @@ -67,8 +53,8 @@ signal_handler(int signal_value, siginfo_t * siginfo, void * context) signal_handler(int signal_value) #endif { - // TODO(wjwwood): remove - std::cout << "signal_handler(" << signal_value << ")" << std::endl; + // TODO(wjwwood): remove? move to console logging at some point? + printf("signal_handler(%d)\n", signal_value); #ifdef HAS_SIGACTION if (old_action.sa_flags & SA_SIGINFO) { if (old_action.sa_sigaction != NULL) { @@ -77,9 +63,9 @@ signal_handler(int signal_value) } else { // *INDENT-OFF* if ( - old_action.sa_handler != NULL && // Is set - old_action.sa_handler != SIG_DFL && // Is not default - old_action.sa_handler != SIG_IGN) // Is not ignored + old_action.sa_handler != NULL && // Is set + old_action.sa_handler != SIG_DFL && // Is not default + old_action.sa_handler != SIG_IGN) // Is not ignored // *INDENT-ON* { old_action.sa_handler(signal_value); @@ -99,23 +85,9 @@ signal_handler(int signal_value) g_is_interrupted.store(true); g_interrupt_condition_variable.notify_all(); } -} // namespace -namespace rclcpp -{ - -RMW_THREAD_LOCAL size_t thread_id = 0; - -namespace utilities -{ - -/// Initialize communications via the rmw implementation and set up a global signal handler. -/** - * \param[in] argc Number of arguments. - * \param[in] argv Argument vector. Will eventually be used for passing options to rclcpp. - */ void -init(int argc, char * argv[]) +rclcpp::utilities::init(int argc, char * argv[]) { (void)argc; (void)argv; @@ -137,21 +109,15 @@ init(int argc, char * argv[]) if (ret == -1) #else ::old_signal_handler = std::signal(SIGINT, ::signal_handler); + // NOLINTNEXTLINE(readability/braces) if (::old_signal_handler == SIG_ERR) #endif { const size_t error_length = 1024; + // NOLINTNEXTLINE(runtime/arrays) char error_string[error_length]; #ifndef _WIN32 - auto rc = strerror_r(errno, error_string, error_length); - if (rc) { - // *INDENT-OFF* - throw std::runtime_error( - "Failed to set SIGINT signal handler: (" + std::to_string(errno) + - ") unable to retrieve error string"); - // *INDENT-ON* - } - + strerror_r(errno, error_string, error_length); #else strerror_s(error_string, error_length, errno); #endif @@ -163,17 +129,14 @@ init(int argc, char * argv[]) } } -/// Check rclcpp's status. -// \return True if SIGINT hasn't fired yet, false otherwise. bool -ok() +rclcpp::utilities::ok() { return ::g_signal_status == 0; } -/// Notify the signal handler and rmw that rclcpp is shutting down. void -shutdown() +rclcpp::utilities::shutdown() { g_signal_status = SIGINT; rmw_ret_t status = rmw_trigger_guard_condition(g_sigint_guard_cond_handle); @@ -185,23 +148,15 @@ shutdown() g_interrupt_condition_variable.notify_all(); } - -/// Get a handle to the rmw guard condition that manages the signal handler. rmw_guard_condition_t * -get_global_sigint_guard_condition() +rclcpp::utilities::get_global_sigint_guard_condition() { return ::g_sigint_guard_cond_handle; } -/// Use the global condition variable to block for the specified amount of time. -/** - * \param[in] nanoseconds A std::chrono::duration representing how long to sleep for. - * \return True if the condition variable did not timeout. - */ bool -sleep_for(const std::chrono::nanoseconds & nanoseconds) +rclcpp::utilities::sleep_for(const std::chrono::nanoseconds & nanoseconds) { - // TODO: determine if posix's nanosleep(2) is more efficient here std::chrono::nanoseconds time_left = nanoseconds; { std::unique_lock lock(::g_interrupt_mutex); @@ -215,12 +170,3 @@ sleep_for(const std::chrono::nanoseconds & nanoseconds) // Return true if the timeout elapsed successfully, otherwise false. return !g_is_interrupted; } - -} /* namespace utilities */ -} /* namespace rclcpp */ - -#ifdef HAS_SIGACTION -#undef HAS_SIGACTION -#endif - -#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */ diff --git a/rclcpp/src/rclcpp/windows_helper.cpp b/rclcpp/src/rclcpp/windows_helper.cpp new file mode 100644 index 0000000..5c72951 --- /dev/null +++ b/rclcpp/src/rclcpp/windows_helper.cpp @@ -0,0 +1,17 @@ +// Copyright 2015 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. + +#if defined(WIN32) +#include "rclcpp/type_support_def.hpp" +#endif diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 6f47c83..dd3c17b 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -118,11 +118,13 @@ public: // Prevent rclcpp/publisher.hpp and rclcpp/subscription.hpp from being imported. #define RCLCPP__PUBLISHER_HPP_ #define RCLCPP__SUBSCRIPTION_HPP_ +#define RCLCPP_BUILDING_LIBRARY 1 // Force ipm to use our mock publisher class. #define Publisher mock::Publisher #define PublisherBase mock::PublisherBase #define SubscriptionBase mock::SubscriptionBase -#include +#include "../src/rclcpp/intra_process_manager.cpp" +#include "../src/rclcpp/intra_process_manager_state.cpp" #undef SubscriptionBase #undef Publisher #undef PublisherBase diff --git a/rclcpp/test/test_mapped_ring_buffer.cpp b/rclcpp/test/test_mapped_ring_buffer.cpp index 3cb6128..93426de 100644 --- a/rclcpp/test/test_mapped_ring_buffer.cpp +++ b/rclcpp/test/test_mapped_ring_buffer.cpp @@ -14,6 +14,7 @@ #include +#define RCLCPP_BUILDING_LIBRARY 1 // Prevent including unavailable symbols #include /*