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/allocator/allocator_deleter.hpp b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp index c3b59f2..ff79c40 100644 --- a/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp +++ b/rclcpp/include/rclcpp/allocator/allocator_deleter.hpp @@ -16,10 +16,10 @@ #define RCLCPP__ALLOCATOR__ALLOCATOR_DELETER_HPP_ #include +#include namespace rclcpp { - namespace allocator { diff --git a/rclcpp/include/rclcpp/any_executable.hpp b/rclcpp/include/rclcpp/any_executable.hpp index 1bdddb8..6f886fd 100644 --- a/rclcpp/include/rclcpp/any_executable.hpp +++ b/rclcpp/include/rclcpp/any_executable.hpp @@ -12,13 +12,14 @@ // 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_ +#ifndef RCLCPP__ANY_EXECUTABLE_HPP_ +#define RCLCPP__ANY_EXECUTABLE_HPP_ #include -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#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; @@ -42,7 +44,7 @@ struct AnyExecutable rclcpp::node::Node::SharedPtr node; }; -} /* executor */ -} /* rclcpp */ +} // namespace executor +} // namespace rclcpp -#endif +#endif // RCLCPP__ANY_EXECUTABLE_HPP_ diff --git a/rclcpp/include/rclcpp/any_service_callback.hpp b/rclcpp/include/rclcpp/any_service_callback.hpp index 8618810..3913672 100644 --- a/rclcpp/include/rclcpp/any_service_callback.hpp +++ b/rclcpp/include/rclcpp/any_service_callback.hpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ -#define RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ - -#include +#ifndef RCLCPP__ANY_SERVICE_CALLBACK_HPP_ +#define RCLCPP__ANY_SERVICE_CALLBACK_HPP_ #include #include +#include #include -#include +#include "rclcpp/function_traits.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/types.h" namespace rclcpp { @@ -97,7 +98,7 @@ public: } }; -} /* namespace any_service_callback */ -} /* namespace rclcpp */ +} // namespace any_service_callback +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_ANY_SERVICE_CALLBACK_HPP_ */ +#endif // RCLCPP__ANY_SERVICE_CALLBACK_HPP_ diff --git a/rclcpp/include/rclcpp/any_subscription_callback.hpp b/rclcpp/include/rclcpp/any_subscription_callback.hpp index 94e26a1..a52e8d9 100644 --- a/rclcpp/include/rclcpp/any_subscription_callback.hpp +++ b/rclcpp/include/rclcpp/any_subscription_callback.hpp @@ -19,10 +19,12 @@ #include #include +#include #include #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..63ebca8 100644 --- a/rclcpp/include/rclcpp/callback_group.hpp +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -12,17 +12,18 @@ // 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_ +#ifndef RCLCPP__CALLBACK_GROUP_HPP_ +#define RCLCPP__CALLBACK_GROUP_HPP_ #include #include #include -#include -#include -#include -#include +#include "rclcpp/client.hpp" +#include "rclcpp/service.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/timer.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -31,7 +32,7 @@ namespace rclcpp namespace node { class Node; -} // namespace node +} // namespace node namespace callback_group { @@ -49,82 +50,61 @@ 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_; - }; -} /* namespace callback_group */ -} /* namespace rclcpp */ +} // namespace callback_group +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */ +#endif // RCLCPP__CALLBACK_GROUP_HPP_ diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 7037363..ba087f4 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -12,60 +12,49 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_CLIENT_HPP_ -#define RCLCPP_RCLCPP_CLIENT_HPP_ +#ifndef RCLCPP__CLIENT_HPP_ +#define RCLCPP__CLIENT_HPP_ #include -#include #include #include #include +#include +#include #include -#include -#include - -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" namespace rclcpp { - namespace client { 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; @@ -79,7 +68,6 @@ private: rmw_client_t * client_handle_; std::string service_name_; - }; template @@ -118,7 +106,8 @@ public: 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 + // TODO(esteve) 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); @@ -158,7 +147,7 @@ private: std::map> pending_requests_; }; -} /* namespace client */ -} /* namespace rclcpp */ +} // namespace client +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_CLIENT_HPP_ */ +#endif // RCLCPP__CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp index 1c56cbb..e06146f 100644 --- a/rclcpp/include/rclcpp/context.hpp +++ b/rclcpp/include/rclcpp/context.hpp @@ -12,20 +12,19 @@ // 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 +#ifndef RCLCPP__CONTEXT_HPP_ +#define RCLCPP__CONTEXT_HPP_ #include - #include #include -#include #include +#include #include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" namespace rclcpp { @@ -38,7 +37,7 @@ class Context public: RCLCPP_SMART_PTR_DEFINITIONS(Context); - Context() {} + Context(); template std::shared_ptr @@ -71,10 +70,9 @@ private: std::unordered_map> sub_contexts_; std::mutex mutex_; - }; -} /* namespace context */ -} /* namespace rclcpp */ +} // namespace context +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */ +#endif // RCLCPP__CONTEXT_HPP_ diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp index c10225e..757a767 100644 --- a/rclcpp/include/rclcpp/contexts/default_context.hpp +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -12,10 +12,11 @@ // 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_ +#ifndef RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ +#define RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ -#include +#include "rclcpp/context.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -29,19 +30,16 @@ 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 -} // namespace rclcpp +} // namespace default_context +} // namespace contexts +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */ +#endif // RCLCPP__CONTEXTS__DEFAULT_CONTEXT_HPP_ 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..310040c 100644 --- a/rclcpp/include/rclcpp/executors.hpp +++ b/rclcpp/include/rclcpp/executors.hpp @@ -12,17 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_ -#define RCLCPP_RCLCPP_EXECUTORS_HPP_ +#ifndef RCLCPP__EXECUTORS_HPP_ +#define RCLCPP__EXECUTORS_HPP_ #include -#include -#include -#include -#include + +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#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 +52,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 +69,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 +95,18 @@ spin_node_until_future_complete( return FutureReturnCode::INTERRUPTED; } -} // namespace executors -} // namespace rclcpp +} // namespace executors -#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ +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__EXECUTORS_HPP_ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 82b1f84..30ca1b0 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -15,18 +15,14 @@ #ifndef RCLCPP__EXECUTORS__MULTI_THREADED_EXECUTOR_HPP_ #define 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" +#include "rclcpp/memory_strategies.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { @@ -40,63 +36,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/function_traits.hpp b/rclcpp/include/rclcpp/function_traits.hpp index 639d8d1..d491202 100644 --- a/rclcpp/include/rclcpp/function_traits.hpp +++ b/rclcpp/include/rclcpp/function_traits.hpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ -#define RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ +#ifndef RCLCPP__FUNCTION_TRAITS_HPP_ +#define RCLCPP__FUNCTION_TRAITS_HPP_ + #include +#include namespace rclcpp { @@ -95,6 +97,7 @@ struct same_arguments : std::is_same< typename function_traits::arguments > {}; -} /* namespace rclcpp */ -#endif /* RCLCPP_RCLCPP_FUNCTION_TRAITS_HPP_ */ +} // namespace rclcpp + +#endif // RCLCPP__FUNCTION_TRAITS_HPP_ 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..0649505 100644 --- a/rclcpp/include/rclcpp/intra_process_manager_state.hpp +++ b/rclcpp/include/rclcpp/intra_process_manager_state.hpp @@ -15,21 +15,23 @@ #ifndef RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_ #define RCLCPP__INTRA_PROCESS_MANAGER_STATE_HPP_ -#include -#include -#include - #include #include #include #include -#include #include -#include +#include #include #include +#include #include +#include "rclcpp/macros.hpp" +#include "rclcpp/mapped_ring_buffer.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/subscription.hpp" +#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/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index c9e5936..b22067d 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_MACROS_HPP_ -#define RCLCPP_RCLCPP_MACROS_HPP_ +#ifndef RCLCPP__MACROS_HPP_ +#define RCLCPP__MACROS_HPP_ /* Disables the copy constructor and operator= for the given class. * @@ -76,6 +76,7 @@ { \ return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ } + /// Defines aliases and static functions for using the Class with unique_ptrs. #define RCLCPP_UNIQUE_PTR_DEFINITIONS(...) \ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ @@ -83,4 +84,4 @@ #define RCLCPP_INFO(Args) std::cout << Args << std::endl; -#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */ +#endif // RCLCPP__MACROS_HPP_ 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..a3950d9 100644 --- a/rclcpp/include/rclcpp/memory_strategies.hpp +++ b/rclcpp/include/rclcpp/memory_strategies.hpp @@ -15,20 +15,17 @@ #ifndef RCLCPP__MEMORY_STRATEGIES_HPP_ #define RCLCPP__MEMORY_STRATEGIES_HPP_ -#include -#include +#include "rclcpp/memory_strategy.hpp" +#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..fc764af 100644 --- a/rclcpp/include/rclcpp/message_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/message_memory_strategy.hpp @@ -16,9 +16,11 @@ #define RCLCPP__MESSAGE_MEMORY_STRATEGY_HPP_ #include +#include #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 index b55d87f..d4d3042 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -31,91 +31,21 @@ #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/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 -using namespace rclcpp; -using namespace node; - -Node::Node(const std::string & node_name, bool use_intra_process_comms) -: Node( - node_name, - rclcpp::contexts::default_context::get_global_default_context(), - use_intra_process_comms) -{} - -Node::Node( - const std::string & node_name, - context::Context::SharedPtr context, - bool use_intra_process_comms) -: name_(node_name), context_(context), - number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), - use_intra_process_comms_(use_intra_process_comms) +namespace rclcpp { - size_t domain_id = 0; - char * ros_domain_id = nullptr; - const char * env_var = "ROS_DOMAIN_ID"; -#ifndef _WIN32 - ros_domain_id = getenv(env_var); -#else - size_t ros_domain_id_size; - _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); -#endif - if (ros_domain_id) { - uint32_t number = strtoul(ros_domain_id, NULL, 0); - if (number == (std::numeric_limits::max)()) { - throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); - } - domain_id = static_cast(number); -#ifdef _WIN32 - free(ros_domain_id); -#endif - } - - auto node = rmw_create_node(name_.c_str(), domain_id); - if (!node) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not create node: ") + - rmw_get_error_string_safe()); - // *INDENT-ON* - } - // Initialize node handle shared_ptr with custom deleter. - // *INDENT-OFF* - node_handle_.reset(node, [](rmw_node_t * node) { - auto ret = rmw_destroy_node(node); - if (ret != RMW_RET_OK) { - fprintf( - stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe()); - } - }); - // *INDENT-ON* - - using rclcpp::callback_group::CallbackGroupType; - default_callback_group_ = create_callback_group( - CallbackGroupType::MutuallyExclusive); - events_publisher_ = create_publisher( - "parameter_events", rmw_qos_profile_parameter_events); -} - -rclcpp::callback_group::CallbackGroup::SharedPtr -Node::create_callback_group( - rclcpp::callback_group::CallbackGroupType group_type) +namespace node { - using rclcpp::callback_group::CallbackGroup; - using rclcpp::callback_group::CallbackGroupType; - auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); - callback_groups_.push_back(group); - return group; -} template typename rclcpp::publisher::Publisher::SharedPtr @@ -157,7 +87,8 @@ Node::create_publisher( 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); + 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( @@ -206,19 +137,6 @@ Node::create_publisher( return publisher; } -bool -Node::group_in_node(callback_group::CallbackGroup::SharedPtr group) -{ - bool group_belongs_to_this_node = false; - for (auto & weak_group : this->callback_groups_) { - auto cur_group = weak_group.lock(); - if (cur_group && (cur_group == group)) { - group_belongs_to_this_node = true; - } - } - return group_belongs_to_this_node; -} - template typename rclcpp::subscription::Subscription::SharedPtr Node::create_subscription( @@ -257,7 +175,8 @@ Node::create_subscription( // *INDENT-ON* } - using namespace rclcpp::subscription; + using rclcpp::subscription::Subscription; + using rclcpp::subscription::SubscriptionBase; auto sub = Subscription::make_shared( node_handle_, @@ -270,7 +189,7 @@ Node::create_subscription( // Setup intra process. if (use_intra_process_comms_) { rmw_subscription_t * intra_process_subscriber_handle = rmw_create_subscription( - node_handle_.get(), ipm_ts_, + 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) @@ -351,39 +270,6 @@ Node::create_subscription( allocator); } -rclcpp::timer::WallTimer::SharedPtr -Node::create_wall_timer( - std::chrono::nanoseconds period, - rclcpp::timer::CallbackType callback, - rclcpp::callback_group::CallbackGroup::SharedPtr group) -{ - auto timer = rclcpp::timer::WallTimer::make_shared(period, callback); - if (group) { - if (!group_in_node(group)) { - // TODO(jacquelinekay): use custom exception - throw std::runtime_error("Cannot create timer, group not in node."); - } - group->add_timer(timer); - } else { - default_callback_group_->add_timer(timer); - } - number_of_timers_++; - return timer; -} - -// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it. -// rclcpp::timer::WallTimer::SharedPtr -// Node::create_wall_timer( -// std::chrono::duration period, -// rclcpp::timer::CallbackType callback, -// rclcpp::callback_group::CallbackGroup::SharedPtr group) -// { -// return create_wall_timer( -// std::chrono::duration_cast(period), -// callback, -// group); -// } - template typename client::Client::SharedPtr Node::create_client( @@ -404,7 +290,8 @@ Node::create_client( // *INDENT-ON* } - using namespace rclcpp::client; + using rclcpp::client::Client; + using rclcpp::client::ClientBase; auto cli = Client::make_shared( node_handle_, @@ -466,211 +353,7 @@ Node::create_service( return serv; } -std::vector -Node::set_parameters( - const std::vector & parameters) -{ - std::vector results; - for (auto p : parameters) { - auto result = set_parameters_atomically({{p}}); - results.push_back(result); - } - return results; -} - -rcl_interfaces::msg::SetParametersResult -Node::set_parameters_atomically( - const std::vector & parameters) -{ - std::lock_guard lock(mutex_); - std::map tmp_map; - auto parameter_event = std::make_shared(); - - for (auto p : parameters) { - if (parameters_.find(p.get_name()) == parameters_.end()) { - if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - parameter_event->new_parameters.push_back(p.to_parameter()); - } - } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { - parameter_event->changed_parameters.push_back(p.to_parameter()); - } else { - parameter_event->deleted_parameters.push_back(p.to_parameter()); - } - tmp_map[p.get_name()] = p; - } - // std::map::insert will not overwrite elements, so we'll keep the new - // ones and add only those that already exist in the Node's internal map - tmp_map.insert(parameters_.begin(), parameters_.end()); - std::swap(tmp_map, parameters_); - - // TODO(jacquelinekay): handle parameter constraints - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - - events_publisher_->publish(parameter_event); - - return result; -} - -std::vector -Node::get_parameters( - const std::vector & names) const -{ - std::lock_guard lock(mutex_); - std::vector results; - - for (auto & name : names) { - if (std::any_of(parameters_.cbegin(), parameters_.cend(), - [&name](const std::pair & kv) { - return name == kv.first; - })) - { - results.push_back(parameters_.at(name)); - } - } - return results; -} - -std::vector -Node::describe_parameters( - const std::vector & names) const -{ - std::lock_guard lock(mutex_); - std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; - parameter_descriptor.name = kv.first; - parameter_descriptor.type = kv.second.get_type(); - results.push_back(parameter_descriptor); - } - } - return results; -} - -std::vector -Node::get_parameter_types( - const std::vector & names) const -{ - std::lock_guard lock(mutex_); - std::vector results; - for (auto & kv : parameters_) { - if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { - return name == kv.first; - })) - { - results.push_back(kv.second.get_type()); - } else { - results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); - } - } - return results; -} - -rcl_interfaces::msg::ListParametersResult -Node::list_parameters( - const std::vector & prefixes, uint64_t depth) const -{ - std::lock_guard lock(mutex_); - rcl_interfaces::msg::ListParametersResult result; - - // TODO(esteve): define parameter separator, use "." for now - for (auto & kv : parameters_) { - if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) { - if (kv.first == prefix) { - return true; - } else if (kv.first.find(prefix + ".") == 0) { - size_t length = prefix.length(); - std::string substr = kv.first.substr(length); - // Cast as unsigned integer to avoid warning - return static_cast(std::count(substr.begin(), substr.end(), '.')) < depth; - } - return false; - })) - { - result.names.push_back(kv.first); - size_t last_separator = kv.first.find_last_of('.'); - if (std::string::npos != last_separator) { - std::string prefix = kv.first.substr(0, last_separator); - if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == - result.prefixes.cend()) - { - result.prefixes.push_back(prefix); - } - } - } - } - return result; -} - -std::map -Node::get_topic_names_and_types() const -{ - rmw_topic_names_and_types_t topic_names_and_types; - topic_names_and_types.topic_count = 0; - topic_names_and_types.topic_names = nullptr; - topic_names_and_types.type_names = nullptr; - - auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not get topic names and types: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - std::map topics; - for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) { - topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i]; - } - - ret = rmw_destroy_topic_names_and_types(&topic_names_and_types); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - - return topics; -} - -size_t -Node::count_publishers(const std::string & topic_name) const -{ - size_t count; - auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not count publishers: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - return count; -} - -size_t -Node::count_subscribers(const std::string & topic_name) const -{ - size_t count; - auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count); - if (ret != RMW_RET_OK) { - // *INDENT-OFF* - throw std::runtime_error( - std::string("could not count subscribers: ") + rmw_get_error_string_safe()); - // *INDENT-ON* - } - return count; -} - - -const Node::CallbackGroupWeakPtrList & -Node::get_callback_groups() const -{ - return callback_groups_; -} +} // 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..271960b 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -12,253 +12,161 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_PARAMETER_HPP_ -#define RCLCPP_RCLCPP_PARAMETER_HPP_ +#ifndef RCLCPP__PARAMETER_HPP_ +#define RCLCPP__PARAMETER_HPP_ #include #include #include +#include -#include - -#include -#include -#include +#include "rcl_interfaces/msg/parameter.hpp" +#include "rcl_interfaces/msg/parameter_type.hpp" +#include "rcl_interfaces/msg/parameter_value.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/rmw.h" namespace rclcpp { - namespace parameter { enum ParameterType { - 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, + 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, }; // Structure to store an arbitrary parameter with templated get/set methods 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 get_value() const { if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { - // TODO: use custom exception + // TODO(wjwwood): 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 + // TODO(wjwwood): 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 + // TODO(wjwwood): 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 + // TODO(wjwwood): 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 + // TODO(wjwwood): use custom exception throw std::runtime_error("Invalid type"); } 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 +175,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 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 */ - -#endif /* RCLCPP_RCLCPP_PARAMETER_HPP_ */ +#endif // RCLCPP__PARAMETER_HPP_ diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 66314bd..3a09139 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -12,220 +12,84 @@ // 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_ +#ifndef RCLCPP__PARAMETER_CLIENT_HPP_ +#define 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 @@ -352,8 +162,7 @@ private: AsyncParametersClient::SharedPtr async_parameters_client_; }; -} /* namespace parameter_client */ +} // namespace parameter_client +} // namespace rclcpp -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_PARAMETER_CLIENT_HPP_ */ +#endif // RCLCPP__PARAMETER_CLIENT_HPP_ diff --git a/rclcpp/include/rclcpp/parameter_service.hpp b/rclcpp/include/rclcpp/parameter_service.hpp index ffc2983..dbb1ce0 100644 --- a/rclcpp/include/rclcpp/parameter_service.hpp +++ b/rclcpp/include/rclcpp/parameter_service.hpp @@ -12,150 +12,36 @@ // 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_ +#ifndef RCLCPP__PARAMETER_SERVICE_HPP_ +#define RCLCPP__PARAMETER_SERVICE_HPP_ #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 { - namespace parameter_service { 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_; @@ -170,8 +56,7 @@ private: rclcpp::service::Service::SharedPtr list_parameters_service_; }; -} /* namespace parameter_service */ +} // namespace parameter_service +} // namespace rclcpp -} /* namespace rclcpp */ - -#endif /* RCLCPP_RCLCPP_PARAMETER_SERVICE_HPP_ */ +#endif // RCLCPP__PARAMETER_SERVICE_HPP_ 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..4f25634 100644 --- a/rclcpp/include/rclcpp/rate.hpp +++ b/rclcpp/include/rclcpp/rate.hpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_RATE_HPP_ -#define RCLCPP_RCLCPP_RATE_HPP_ +#ifndef RCLCPP__RATE_HPP_ +#define RCLCPP__RATE_HPP_ #include #include #include -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/utilities.hpp" +#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()) {} @@ -109,13 +110,12 @@ private: std::chrono::nanoseconds period_; std::chrono::time_point last_interval_; - }; using Rate = GenericRate; using WallRate = GenericRate; -} // namespace rate -} // namespace rclcpp +} // namespace rate +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_RATE_HPP_ */ +#endif // RCLCPP__RATE_HPP_ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index c91d31a..b96da67 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -12,22 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_ -#define RCLCPP_RCLCPP_RCLCPP_HPP_ +#ifndef RCLCPP__RCLCPP_HPP_ +#define RCLCPP__RCLCPP_HPP_ #include #include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/parameter.hpp" +#include "rclcpp/parameter_client.hpp" +#include "rclcpp/parameter_service.hpp" +#include "rclcpp/rate.hpp" +#include "rclcpp/utilities.hpp" +#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); -} +} // namespace rclcpp -/// 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_ */ +#endif // RCLCPP__RCLCPP_HPP_ diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 2bfbb09..84c2c99 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_SERVICE_HPP_ -#define RCLCPP_RCLCPP_SERVICE_HPP_ +#ifndef RCLCPP__SERVICE_HPP_ +#define RCLCPP__SERVICE_HPP_ #include #include @@ -21,52 +21,38 @@ #include #include -#include -#include - -#include -#include +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" namespace rclcpp { - namespace service { 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; @@ -81,10 +67,9 @@ private: rmw_service_t * service_handle_; std::string service_name_; - }; -using namespace any_service_callback; +using any_service_callback::AnyServiceCallback; template class Service : public ServiceBase @@ -152,7 +137,7 @@ private: AnyServiceCallback any_callback_; }; -} /* namespace service */ -} /* namespace rclcpp */ +} // namespace service +} // namespace rclcpp -#endif /* RCLCPP_RCLCPP_SERVICE_HPP_ */ +#endif // RCLCPP__SERVICE_HPP_ diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 7622123..3824bf6 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -21,13 +21,12 @@ #include "rclcpp/allocator/allocator_common.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/visibility_control.hpp" namespace rclcpp { - namespace memory_strategies { - namespace allocator_memory_strategy { @@ -312,7 +311,6 @@ private: } // namespace allocator_memory_strategy } // namespace memory_strategies - } // namespace rclcpp #endif // RCLCPP__STRATEGIES__ALLOCATOR_MEMORY_STRATEGY_HPP_ diff --git a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp index 8f9438c..0a7ce22 100644 --- a/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/message_pool_memory_strategy.hpp @@ -15,8 +15,9 @@ #ifndef RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ #define RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/message_memory_strategy.hpp" +#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 { @@ -101,4 +106,5 @@ protected: } // namespace message_pool_memory_strategy } // namespace strategies } // namespace rclcpp + #endif // RCLCPP__STRATEGIES__MESSAGE_POOL_MEMORY_STRATEGY_HPP_ diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index fb9c952..0044b20 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..e418b79 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_TIMER_HPP_ -#define RCLCPP_RCLCPP_TIMER_HPP_ +#ifndef RCLCPP__TIMER_HPP_ +#define RCLCPP__TIMER_HPP_ #include #include @@ -21,16 +21,15 @@ #include #include -#include -#include - -#include -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/rate.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" namespace rclcpp { - namespace timer { @@ -38,36 +37,26 @@ using CallbackType = std::function; 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. @@ -91,14 +80,12 @@ protected: 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); @@ -166,7 +153,6 @@ private: rclcpp::rate::GenericRate loop_rate_; std::chrono::time_point last_triggered_time_; - }; using WallTimer = GenericTimer; @@ -174,4 +160,4 @@ using WallTimer = GenericTimer; } /* namespace timer */ } /* namespace rclcpp */ -#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */ +#endif // RCLCPP__TIMER_HPP_ 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..1006152 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -12,100 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ -#define RCLCPP_RCLCPP_UTILITIES_HPP_ +#ifndef RCLCPP__UTILITIES_HPP_ +#define 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 */ +} // namespace utilities +} // namespace rclcpp -#ifdef HAS_SIGACTION -#undef HAS_SIGACTION -#endif - -#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */ +#endif // 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 new file mode 100644 index 0000000..0566140 --- /dev/null +++ b/rclcpp/src/rclcpp/any_executable.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "rclcpp/any_executable.hpp" + +using rclcpp::executor::AnyExecutable; + +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 new file mode 100644 index 0000000..2555973 --- /dev/null +++ b/rclcpp/src/rclcpp/callback_group.cpp @@ -0,0 +1,85 @@ +// 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. + +#include "rclcpp/callback_group.hpp" + +#include + +using rclcpp::callback_group::CallbackGroup; +using rclcpp::callback_group::CallbackGroupType; + +CallbackGroup::CallbackGroup(CallbackGroupType group_type) +: type_(group_type), can_be_taken_from_(true) +{} + +const std::vector & +CallbackGroup::get_subscription_ptrs() const +{ + return subscription_ptrs_; +} + +const std::vector & +CallbackGroup::get_timer_ptrs() const +{ + return timer_ptrs_; +} + +const std::vector & +CallbackGroup::get_service_ptrs() const +{ + return service_ptrs_; +} + +const std::vector & +CallbackGroup::get_client_ptrs() const +{ + return client_ptrs_; +} + +std::atomic_bool & +CallbackGroup::can_be_taken_from() +{ + return can_be_taken_from_; +} + +const CallbackGroupType & +CallbackGroup::type() const +{ + return type_; +} + +void +CallbackGroup::add_subscription( + const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr) +{ + subscription_ptrs_.push_back(subscription_ptr); +} + +void +CallbackGroup::add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr) +{ + timer_ptrs_.push_back(timer_ptr); +} + +void +CallbackGroup::add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr) +{ + service_ptrs_.push_back(service_ptr); +} + +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 new file mode 100644 index 0000000..29aa62a --- /dev/null +++ b/rclcpp/src/rclcpp/client.cpp @@ -0,0 +1,51 @@ +// 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. + +#include "rclcpp/client.hpp" + +#include +#include + +#include "rmw/rmw.h" + +using rclcpp::client::ClientBase; + +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() +{ + 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 & +ClientBase::get_service_name() const +{ + return this->service_name_; +} + +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 new file mode 100644 index 0000000..a5f3c17 --- /dev/null +++ b/rclcpp/src/rclcpp/context.cpp @@ -0,0 +1,19 @@ +// 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. + +#include "rclcpp/context.hpp" + +using rclcpp::context::Context; + +Context::Context() {} diff --git a/rclcpp/src/rclcpp/contexts/default_context.cpp b/rclcpp/src/rclcpp/contexts/default_context.cpp new file mode 100644 index 0000000..53c6c9f --- /dev/null +++ b/rclcpp/src/rclcpp/contexts/default_context.cpp @@ -0,0 +1,27 @@ +// 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. + +#include "rclcpp/contexts/default_context.hpp" + +using rclcpp::contexts::default_context::DefaultContext; + +DefaultContext::DefaultContext() +{} + +DefaultContext::SharedPtr +rclcpp::contexts::default_context::get_global_default_context() +{ + static DefaultContext::SharedPtr default_context = DefaultContext::make_shared(); + return default_context; +} diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp new file mode 100644 index 0000000..cf15e98 --- /dev/null +++ b/rclcpp/src/rclcpp/executor.cpp @@ -0,0 +1,518 @@ +// 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. + +#include "rclcpp/executor.hpp" + +#include "rcl_interfaces/msg/intra_process_message.hpp" + +using rclcpp::executor::AnyExecutable; +using rclcpp::executor::Executor; + +Executor::Executor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) +: interrupt_guard_condition_(rmw_create_guard_condition()), + memory_strategy_(ms) +{ +} + +Executor::~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()); + } + } +} + +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 + 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()); + } + } + } +} + +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())) + { + execute_any_executable(any_exec); + } +} + +void +Executor::set_memory_strategy(rclcpp::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( + remove_if( + weak_nodes_.begin(), weak_nodes_.end(), + // *INDENT-OFF* (prevent uncrustify from making unecessary indents here) + [](std::weak_ptr i) + { + return i.expired(); + } + // *INDENT-ON* + ) + ); + } + + // 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::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 +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_timer : group->get_timer_ptrs()) { + auto t = weak_timer.lock(); + if (t == timer) { + return group; + } + } + } + } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); +} + +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 & 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 +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 & 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 +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; +} + +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 new file mode 100644 index 0000000..49d4197 --- /dev/null +++ b/rclcpp/src/rclcpp/executors.cpp @@ -0,0 +1,30 @@ +// 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. + +#include "rclcpp/executors.hpp" + +void +rclcpp::spin_some(node::Node::SharedPtr node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.spin_node_some(node_ptr); +} + +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 new file mode 100644 index 0000000..5684213 --- /dev/null +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -0,0 +1,75 @@ +// 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. + +#include "rclcpp/executors/multi_threaded_executor.hpp" + +#include +#include +#include + +#include "rclcpp/utilities.hpp" + +using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; + +MultiThreadedExecutor::MultiThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) +: executor::Executor(ms) +{ + number_of_threads_ = std::thread::hardware_concurrency(); + if (number_of_threads_ == 0) { + number_of_threads_ = 1; + } +} + +MultiThreadedExecutor::~MultiThreadedExecutor() {} + +void +MultiThreadedExecutor::spin() +{ + std::vector threads; + { + 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(); + } +} + +size_t +MultiThreadedExecutor::get_number_of_threads() +{ + return number_of_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_); + if (!rclcpp::utilities::ok()) { + return; + } + any_exec = get_next_executable(); + } + execute_any_executable(any_exec); + } +} diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp new file mode 100644 index 0000000..23001fe --- /dev/null +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -0,0 +1,32 @@ +// 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. + +#include + +using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; + +SingleThreadedExecutor::SingleThreadedExecutor(rclcpp::memory_strategy::MemoryStrategy::SharedPtr ms) +: executor::Executor(ms) {} + + +SingleThreadedExecutor::~SingleThreadedExecutor() {} + +void +SingleThreadedExecutor::spin() +{ + while (rclcpp::utilities::ok()) { + auto any_exec = get_next_executable(); + execute_any_executable(any_exec); + } +} diff --git a/rclcpp/src/rclcpp/intra_process_manager.cpp b/rclcpp/src/rclcpp/intra_process_manager.cpp new file mode 100644 index 0000000..b919f4c --- /dev/null +++ b/rclcpp/src/rclcpp/intra_process_manager.cpp @@ -0,0 +1,82 @@ +// 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. + +#include "rclcpp/intra_process_manager.hpp" + +namespace rclcpp +{ +namespace intra_process_manager +{ + +static std::atomic _next_unique_id {1}; + +IntraProcessManager::IntraProcessManager( + rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr state) +: state_(state) +{} + +IntraProcessManager::~IntraProcessManager() +{} + +uint64_t +IntraProcessManager::add_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr subscription) +{ + 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); +} + +void +IntraProcessManager::remove_publisher(uint64_t intra_process_publisher_id) +{ + state_->remove_publisher(intra_process_publisher_id); +} + +bool +IntraProcessManager::matches_any_publishers(const rmw_gid_t * id) const +{ + return state_->matches_any_publishers(id); +} + +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* + } + return next_id; +} + +} // namespace intra_process_manager +} // namespace rclcpp diff --git a/rclcpp/src/rclcpp/intra_process_manager_state.cpp b/rclcpp/src/rclcpp/intra_process_manager_state.cpp new file mode 100644 index 0000000..113623f --- /dev/null +++ b/rclcpp/src/rclcpp/intra_process_manager_state.cpp @@ -0,0 +1,23 @@ +// 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. + +#include "rclcpp/intra_process_manager_state.hpp" + +#include + +rclcpp::intra_process_manager::IntraProcessManagerStateBase::SharedPtr +rclcpp::intra_process_manager::create_default_state() +{ + return std::make_shared>(); +} diff --git a/rclcpp/src/rclcpp/memory_strategies.cpp b/rclcpp/src/rclcpp/memory_strategies.cpp new file mode 100644 index 0000000..754b8bb --- /dev/null +++ b/rclcpp/src/rclcpp/memory_strategies.cpp @@ -0,0 +1,25 @@ +// 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. + +#include "rclcpp/memory_strategies.hpp" + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; + +rclcpp::memory_strategy::MemoryStrategy::SharedPtr +rclcpp::memory_strategies::create_default_strategy() +{ + return std::make_shared>(); +} diff --git a/rclcpp/src/rclcpp/memory_strategy.cpp b/rclcpp/src/rclcpp/memory_strategy.cpp new file mode 100644 index 0000000..28c4bfd --- /dev/null +++ b/rclcpp/src/rclcpp/memory_strategy.cpp @@ -0,0 +1,193 @@ +// 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. + +#include "rclcpp/memory_strategy.hpp" + +using rclcpp::memory_strategy::MemoryStrategy; + +rclcpp::subscription::SubscriptionBase::SharedPtr +MemoryStrategy::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; +} + +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; +} + +rclcpp::client::ClientBase::SharedPtr +MemoryStrategy::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; +} + +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; +} + +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_sub : group->get_subscription_ptrs()) { + auto sub = weak_sub.lock(); + if (sub == subscription) { + return group; + } + } + } + } + return nullptr; +} + +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 & serv : group->get_service_ptrs()) { + if (serv == service) { + return 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_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; +} diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp new file mode 100644 index 0000000..b581f33 --- /dev/null +++ b/rclcpp/src/rclcpp/node.cpp @@ -0,0 +1,392 @@ +// 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. + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/node.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 +{ + +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 rclcpp::node::Node; + +Node::Node(const std::string & node_name, bool use_intra_process_comms) +: Node( + node_name, + rclcpp::contexts::default_context::get_global_default_context(), + use_intra_process_comms) +{} + +Node::Node( + const std::string & node_name, + rclcpp::context::Context::SharedPtr context, + bool use_intra_process_comms) +: name_(node_name), context_(context), + number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0), + use_intra_process_comms_(use_intra_process_comms) +{ + size_t domain_id = 0; + char * ros_domain_id = nullptr; + const char * env_var = "ROS_DOMAIN_ID"; +#ifndef _WIN32 + ros_domain_id = getenv(env_var); +#else + size_t ros_domain_id_size; + _dupenv_s(&ros_domain_id, &ros_domain_id_size, env_var); +#endif + if (ros_domain_id) { + uint32_t number = strtoul(ros_domain_id, NULL, 0); + if (number == (std::numeric_limits::max)()) { + throw std::runtime_error("failed to interpret ROS_DOMAIN_ID as integral number"); + } + domain_id = static_cast(number); +#ifdef _WIN32 + free(ros_domain_id); +#endif + } + + auto node = rmw_create_node(name_.c_str(), domain_id); + if (!node) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not create node: ") + + rmw_get_error_string_safe()); + // *INDENT-ON* + } + // Initialize node handle shared_ptr with custom deleter. + // *INDENT-OFF* + node_handle_.reset(node, [](rmw_node_t * node) { + auto ret = rmw_destroy_node(node); + if (ret != RMW_RET_OK) { + fprintf( + stderr, "Error in destruction of rmw node handle: %s\n", rmw_get_error_string_safe()); + } + }); + // *INDENT-ON* + + using rclcpp::callback_group::CallbackGroupType; + default_callback_group_ = create_callback_group( + CallbackGroupType::MutuallyExclusive); + events_publisher_ = create_publisher( + "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) +{ + using rclcpp::callback_group::CallbackGroup; + using rclcpp::callback_group::CallbackGroupType; + auto group = CallbackGroup::SharedPtr(new CallbackGroup(group_type)); + callback_groups_.push_back(group); + return group; +} + +bool +Node::group_in_node(rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + bool group_belongs_to_this_node = false; + for (auto & weak_group : this->callback_groups_) { + auto cur_group = weak_group.lock(); + if (cur_group && (cur_group == group)) { + group_belongs_to_this_node = true; + } + } + return group_belongs_to_this_node; +} + +rclcpp::timer::WallTimer::SharedPtr +Node::create_wall_timer( + std::chrono::nanoseconds period, + rclcpp::timer::CallbackType callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group) +{ + auto timer = rclcpp::timer::WallTimer::make_shared(period, callback); + if (group) { + if (!group_in_node(group)) { + // TODO(jacquelinekay): use custom exception + throw std::runtime_error("Cannot create timer, group not in node."); + } + group->add_timer(timer); + } else { + default_callback_group_->add_timer(timer); + } + number_of_timers_++; + return timer; +} + +// TODO(wjwwood): reenable this once I figure out why the demo doesn't build with it. +// rclcpp::timer::WallTimer::SharedPtr +// Node::create_wall_timer( +// std::chrono::duration period, +// rclcpp::timer::CallbackType callback, +// rclcpp::callback_group::CallbackGroup::SharedPtr group) +// { +// return create_wall_timer( +// std::chrono::duration_cast(period), +// callback, +// group); +// } + +std::vector +Node::set_parameters( + const std::vector & parameters) +{ + std::vector results; + for (auto p : parameters) { + auto result = set_parameters_atomically({{p}}); + results.push_back(result); + } + return results; +} + +rcl_interfaces::msg::SetParametersResult +Node::set_parameters_atomically( + const std::vector & parameters) +{ + std::lock_guard lock(mutex_); + std::map tmp_map; + auto parameter_event = std::make_shared(); + + for (auto p : parameters) { + if (parameters_.find(p.get_name()) == parameters_.end()) { + if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { + parameter_event->new_parameters.push_back(p.to_parameter()); + } + } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) { + parameter_event->changed_parameters.push_back(p.to_parameter()); + } else { + parameter_event->deleted_parameters.push_back(p.to_parameter()); + } + tmp_map[p.get_name()] = p; + } + // std::map::insert will not overwrite elements, so we'll keep the new + // ones and add only those that already exist in the Node's internal map + tmp_map.insert(parameters_.begin(), parameters_.end()); + std::swap(tmp_map, parameters_); + + // TODO(jacquelinekay): handle parameter constraints + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + events_publisher_->publish(parameter_event); + + return result; +} + +std::vector +Node::get_parameters( + const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + + for (auto & name : names) { + if (std::any_of(parameters_.cbegin(), parameters_.cend(), + [&name](const std::pair & kv) { + return name == kv.first; + })) + { + results.push_back(parameters_.at(name)); + } + } + return results; +} + +std::vector +Node::describe_parameters( + const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + rcl_interfaces::msg::ParameterDescriptor parameter_descriptor; + parameter_descriptor.name = kv.first; + parameter_descriptor.type = kv.second.get_type(); + results.push_back(parameter_descriptor); + } + } + return results; +} + +std::vector +Node::get_parameter_types( + const std::vector & names) const +{ + std::lock_guard lock(mutex_); + std::vector results; + for (auto & kv : parameters_) { + if (std::any_of(names.cbegin(), names.cend(), [&kv](const std::string & name) { + return name == kv.first; + })) + { + results.push_back(kv.second.get_type()); + } else { + results.push_back(rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + } + } + return results; +} + +rcl_interfaces::msg::ListParametersResult +Node::list_parameters( + const std::vector & prefixes, uint64_t depth) const +{ + std::lock_guard lock(mutex_); + rcl_interfaces::msg::ListParametersResult result; + + // TODO(esteve): define parameter separator, use "." for now + for (auto & kv : parameters_) { + if (std::any_of(prefixes.cbegin(), prefixes.cend(), [&kv, &depth](const std::string & prefix) { + if (kv.first == prefix) { + return true; + } else if (kv.first.find(prefix + ".") == 0) { + size_t length = prefix.length(); + std::string substr = kv.first.substr(length); + // Cast as unsigned integer to avoid warning + return static_cast(std::count(substr.begin(), substr.end(), '.')) < depth; + } + return false; + })) + { + result.names.push_back(kv.first); + size_t last_separator = kv.first.find_last_of('.'); + if (std::string::npos != last_separator) { + std::string prefix = kv.first.substr(0, last_separator); + if (std::find(result.prefixes.cbegin(), result.prefixes.cend(), prefix) == + result.prefixes.cend()) + { + result.prefixes.push_back(prefix); + } + } + } + } + return result; +} + +std::map +Node::get_topic_names_and_types() const +{ + rmw_topic_names_and_types_t topic_names_and_types; + topic_names_and_types.topic_count = 0; + topic_names_and_types.topic_names = nullptr; + topic_names_and_types.type_names = nullptr; + + auto ret = rmw_get_topic_names_and_types(node_handle_.get(), &topic_names_and_types); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not get topic names and types: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + std::map topics; + for (size_t i = 0; i < topic_names_and_types.topic_count; ++i) { + topics[topic_names_and_types.topic_names[i]] = topic_names_and_types.type_names[i]; + } + + ret = rmw_destroy_topic_names_and_types(&topic_names_and_types); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not destroy topic names and types: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + + return topics; +} + +size_t +Node::count_publishers(const std::string & topic_name) const +{ + size_t count; + auto ret = rmw_count_publishers(node_handle_.get(), topic_name.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count publishers: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + return count; +} + +size_t +Node::count_subscribers(const std::string & topic_name) const +{ + size_t count; + auto ret = rmw_count_subscribers(node_handle_.get(), topic_name.c_str(), &count); + if (ret != RMW_RET_OK) { + // *INDENT-OFF* + throw std::runtime_error( + std::string("could not count subscribers: ") + rmw_get_error_string_safe()); + // *INDENT-ON* + } + return count; +} + + +const Node::CallbackGroupWeakPtrList & +Node::get_callback_groups() const +{ + return callback_groups_; +} diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp new file mode 100644 index 0000000..0e8fe5f --- /dev/null +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -0,0 +1,278 @@ +// 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. + +#include "rclcpp/parameter.hpp" + +#include +#include +#include +#include + +using rclcpp::parameter::ParameterType; +using rclcpp::parameter::ParameterVariant; + +ParameterVariant::ParameterVariant() +: name_("") +{ + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET; +} + +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; +} + +ParameterVariant::ParameterVariant(const std::string & name, const int int_value) +: name_(name) +{ + value_.integer_value = int_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; +} + +ParameterVariant::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; +} + +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; +} + +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* + } +} + +const std::string & +ParameterVariant::get_name() const +{ + return name_; +} + +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* + } +} + +rcl_interfaces::msg::Parameter +ParameterVariant::to_parameter() +{ + rcl_interfaces::msg::Parameter parameter; + parameter.name = name_; + parameter.value = value_; + return parameter; +} + +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* + } +} + +std::string +rclcpp::parameter::_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(); +} + +std::ostream & +rclcpp::parameter::operator<<(std::ostream & os, const rclcpp::parameter::ParameterVariant & pv) +{ + 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() << "\", "; + ss << "\"type\": \"" << param.get_type_name() << "\", "; + ss << "\"value\": \"" << param.value_to_string() << "\"}"; + return ss.str(); +} + +std::string +std::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(); +} diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp new file mode 100644 index 0000000..9abe83e --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -0,0 +1,366 @@ +// 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. + +#include "rclcpp/parameter_client.hpp" + +#include +#include +#include + +// 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 +{ + +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_get_parameters_srv_type_support(); +} + +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_get_parameter_types_srv_type_support(); +} + +template<> +const rosidl_service_type_support_t * +get_service_type_support_handle() +{ + return rclcpp::type_support::get_set_parameters_srv_type_support(); +} + +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"); +} + +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(); + + 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) + { + 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); + } + } + ); + // *INDENT-ON* + + return future_result; +} + +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) + { + 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); + } + } + ); + // *INDENT-ON* + + return future_result; +} + +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) + { + promise_result->set_value(cb_f.get()->results); + if (callback != nullptr) { + callback(future_result); + } + } + ); + // *INDENT-ON* + + return future_result; +} + +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) + { + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } + } + ); + // *INDENT-ON* + + return future_result; +} + +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(); + + 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) + { + promise_result->set_value(cb_f.get()->result); + if (callback != nullptr) { + callback(future_result); + } + } + ); + // *INDENT-ON* + + return future_result; +} + +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 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(); + } + + throw std::runtime_error("Unable to get result of set parameters service call."); +} + +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); + + 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."); +} diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp new file mode 100644 index 0000000..e1efa70 --- /dev/null +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -0,0 +1,140 @@ +// 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. + +#include "rclcpp/parameter_service.hpp" + +#include +#include + +using rclcpp::parameter_service::ParameterService; + +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()); + } + } + ); + + 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; + } + ); + // *INDENT-ON* +} diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp new file mode 100644 index 0000000..4ad9b87 --- /dev/null +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -0,0 +1,141 @@ +// 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. + +#include "rclcpp/publisher.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#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" + +using rclcpp::publisher::PublisherBase; + +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) +{ + // 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* + } +} + +PublisherBase::~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()); + } + } +} + +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()); + } + 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; +} + +void +PublisherBase::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* + } +} diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp new file mode 100644 index 0000000..f2f588c --- /dev/null +++ b/rclcpp/src/rclcpp/service.cpp @@ -0,0 +1,59 @@ +// 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. + +#include "rclcpp/service.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/any_service_callback.hpp" +#include "rclcpp/macros.hpp" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +using rclcpp::service::ServiceBase; + +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() +{ + 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 +ServiceBase::get_service_name() +{ + return this->service_name_; +} + +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 new file mode 100644 index 0000000..caf6578 --- /dev/null +++ b/rclcpp/src/rclcpp/subscription.cpp @@ -0,0 +1,78 @@ +// 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. + +#include "rclcpp/subscription.hpp" + +#include +#include + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + + +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_; +} + +SubscriptionBase::~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(); + } + } +} + +const std::string & +SubscriptionBase::get_topic_name() const +{ + return this->topic_name_; +} + +const rmw_subscription_t * +SubscriptionBase::get_subscription_handle() const +{ + return subscription_handle_; +} + +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 new file mode 100644 index 0000000..2ee84ee --- /dev/null +++ b/rclcpp/src/rclcpp/timer.cpp @@ -0,0 +1,47 @@ +// 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. + +#include "rclcpp/timer.hpp" + +#include + +using rclcpp::timer::CallbackType; +using rclcpp::timer::TimerBase; + +TimerBase::TimerBase(std::chrono::nanoseconds period, CallbackType callback) +: period_(period), + callback_(callback), + canceled_(false) +{} + +TimerBase::~TimerBase() +{} + +void +TimerBase::cancel() +{ + this->canceled_ = true; +} + +void +TimerBase::execute_callback() const +{ + callback_(); +} + +const CallbackType & +TimerBase::get_callback() const +{ + return callback_; +} diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp new file mode 100644 index 0000000..4ae1b55 --- /dev/null +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -0,0 +1,172 @@ +// 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. + +#include "rclcpp/utilities.hpp" + +#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 + +/// Represent the status of the global interrupt signal. +static volatile sig_atomic_t g_signal_status = 0; +/// Guard condition for interrupting the rmw implementation when the global interrupt signal fired. +static rmw_guard_condition_t * g_sigint_guard_cond_handle = rmw_create_guard_condition(); +/// Condition variable for timed sleep (see sleep_for). +static std::condition_variable g_interrupt_condition_variable; +static std::atomic g_is_interrupted(false); +/// Mutex for protecting the global condition variable. +static std::mutex g_interrupt_mutex; + +#ifdef HAS_SIGACTION +static struct sigaction old_action; +#else +static void (* old_signal_handler)(int) = 0; +#endif + +void +#ifdef HAS_SIGACTION +signal_handler(int signal_value, siginfo_t * siginfo, void * context) +#else +signal_handler(int signal_value) +#endif +{ + // 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) { + 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(); +} + +void +rclcpp::utilities::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); + // 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 + strerror_r(errno, error_string, error_length); +#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* + } +} + +bool +rclcpp::utilities::ok() +{ + return ::g_signal_status == 0; +} + +void +rclcpp::utilities::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(); +} + +rmw_guard_condition_t * +rclcpp::utilities::get_global_sigint_guard_condition() +{ + return ::g_sigint_guard_cond_handle; +} + +bool +rclcpp::utilities::sleep_for(const std::chrono::nanoseconds & nanoseconds) +{ + 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; +} 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_function_traits.cpp b/rclcpp/test/test_function_traits.cpp index 0eb2c7f..e92420e 100644 --- a/rclcpp/test/test_function_traits.cpp +++ b/rclcpp/test/test_function_traits.cpp @@ -21,6 +21,7 @@ int func_no_args() return 0; } +// NOLINTNEXTLINE(readability/casting) int func_one_int(int) { return 1; @@ -142,15 +143,20 @@ TEST(TestFunctionTraits, arity) { return 0; }; - auto lambda_one_int = [](int) { + auto lambda_one_int = [](int one) { + (void)one; return 1; }; - auto lambda_two_ints = [](int, int) { + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; return 2; }; - auto lambda_one_int_one_char = [](int, char) { + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; return 3; }; @@ -224,15 +230,20 @@ TEST(TestFunctionTraits, argument_types) { >::value, "Functor accepts a char as second argument"); // Test lambdas - auto lambda_one_int = [](int) { + auto lambda_one_int = [](int one) { + (void)one; return 1; }; - auto lambda_two_ints = [](int, int) { + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; return 2; }; - auto lambda_one_int_one_char = [](int, char) { + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; return 3; }; @@ -336,15 +347,20 @@ TEST(TestFunctionTraits, check_arguments) { "Functor accepts an int and a char as arguments"); // Test lambdas - auto lambda_one_int = [](int) { + auto lambda_one_int = [](int one) { + (void)one; return 1; }; - auto lambda_two_ints = [](int, int) { + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; return 2; }; - auto lambda_one_int_one_char = [](int, char) { + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; return 3; }; @@ -378,11 +394,14 @@ TEST(TestFunctionTraits, check_arguments) { Tests that same_arguments work. */ TEST(TestFunctionTraits, same_arguments) { - auto lambda_one_int = [](int) { + auto lambda_one_int = [](int one) { + (void)one; return 1; }; - auto lambda_two_ints = [](int, int) { + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; return 1; }; @@ -427,15 +446,20 @@ TEST(TestFunctionTraits, sfinae_match) { return 0; }; - auto lambda_one_int = [](int) { + auto lambda_one_int = [](int one) { + (void)one; return 1; }; - auto lambda_two_ints = [](int, int) { + auto lambda_two_ints = [](int one, int two) { + (void)one; + (void)two; return 2; }; - auto lambda_one_int_one_char = [](int, char) { + auto lambda_one_int_one_char = [](int one, char two) { + (void)one; + (void)two; return 3; }; diff --git a/rclcpp/test/test_intra_process_manager.cpp b/rclcpp/test/test_intra_process_manager.cpp index 6f47c83..6d511d9 100644 --- a/rclcpp/test/test_intra_process_manager.cpp +++ b/rclcpp/test/test_intra_process_manager.cpp @@ -13,12 +13,12 @@ // limitations under the License. #include +#include -#include - -#include -#include -#include +#include "gtest/gtest.h" +#include "rclcpp/allocator/allocator_common.hpp" +#include "rclcpp/macros.hpp" +#include "rmw/types.h" // Mock up publisher and subscription base to avoid needing an rmw impl. namespace rclcpp @@ -79,9 +79,9 @@ public: } }; -} -} -} +} // namespace mock +} // namespace publisher +} // namespace rclcpp namespace rclcpp { @@ -111,22 +111,25 @@ public: size_t mock_queue_size; }; -} -} -} +} // namespace mock +} // namespace subscription +} // namespace rclcpp // 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 +// NOLINTNEXTLINE(build/include_order) #include /* @@ -142,13 +145,15 @@ public: TEST(TestIntraProcessManager, nominal) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; - auto p2 = - std::make_shared>(); + auto p2 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p2->mock_topic_name = "nominal2"; p2->mock_queue_size = 10; @@ -231,8 +236,9 @@ TEST(TestIntraProcessManager, nominal) { TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -272,8 +278,9 @@ TEST(TestIntraProcessManager, remove_publisher_before_trying_to_take) { TEST(TestIntraProcessManager, removed_subscription_affects_take) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -342,8 +349,9 @@ TEST(TestIntraProcessManager, removed_subscription_affects_take) { TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; @@ -413,18 +421,21 @@ TEST(TestIntraProcessManager, multiple_subscriptions_one_publisher) { TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; - auto p2 = - std::make_shared>(); + auto p2 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p2->mock_topic_name = "nominal1"; p2->mock_queue_size = 10; - auto p3 = - std::make_shared>(); + auto p3 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p3->mock_topic_name = "nominal1"; p3->mock_queue_size = 10; @@ -510,18 +521,21 @@ TEST(TestIntraProcessManager, multiple_publishers_one_subscription) { TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 10; - auto p2 = - std::make_shared>(); + auto p2 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p2->mock_topic_name = "nominal1"; p2->mock_queue_size = 10; - auto p3 = - std::make_shared>(); + auto p3 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p3->mock_topic_name = "nominal1"; p3->mock_queue_size = 10; @@ -674,8 +688,9 @@ TEST(TestIntraProcessManager, multiple_publishers_multiple_subscription) { TEST(TestIntraProcessManager, ring_buffer_displacement) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -744,8 +759,9 @@ TEST(TestIntraProcessManager, ring_buffer_displacement) { TEST(TestIntraProcessManager, subscription_creation_race_condition) { rclcpp::intra_process_manager::IntraProcessManager ipm; - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -792,8 +808,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_take) { uint64_t p1_id; uint64_t p1_m1_id; { - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; @@ -830,8 +847,9 @@ TEST(TestIntraProcessManager, publisher_out_of_scope_store) { uint64_t p1_id; { - auto p1 = - std::make_shared>(); + auto p1 = std::make_shared< + rclcpp::publisher::mock::Publisher + >(); p1->mock_topic_name = "nominal1"; p1->mock_queue_size = 2; 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 /*