From 35cf3952e3ea819f86c22271b5343a5498fc60cc Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 3 Mar 2015 22:11:59 -0800 Subject: [PATCH] updated to use new C rmw api --- rclcpp/include/rclcpp/executor.hpp | 113 +++++++++--------- .../executors/multi_threaded_executor.hpp | 3 +- .../executors/single_threaded_executor.hpp | 3 +- rclcpp/include/rclcpp/node.hpp | 5 +- rclcpp/include/rclcpp/node_impl.hpp | 33 +++-- rclcpp/include/rclcpp/publisher.hpp | 9 +- rclcpp/include/rclcpp/subscription.hpp | 9 +- rclcpp/include/rclcpp/timer.hpp | 9 +- rclcpp/include/rclcpp/utilities.hpp | 19 ++- 9 files changed, 99 insertions(+), 104 deletions(-) diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 4b1725b..132690b 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -39,11 +39,16 @@ class Executor public: RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); - Executor() - : interrupt_guard_condition_( - ros_middleware_interface::create_guard_condition()) - {} - virtual ~Executor() {} + Executor() : interrupt_guard_condition_(rmw_create_guard_condition()) {} + + virtual ~Executor() + { + if (interrupt_guard_condition_ != nullptr) + { + // TODO(wjwwood): Check ret code. + rmw_destroy_guard_condition(interrupt_guard_condition_); + } + } virtual void spin() = 0; @@ -63,8 +68,7 @@ public: } weak_nodes_.push_back(node_ptr); // Interrupt waiting to handle new node - using ros_middleware_interface::trigger_guard_condition; - trigger_guard_condition(interrupt_guard_condition_); + rmw_trigger_guard_condition(interrupt_guard_condition_); } virtual void @@ -82,8 +86,7 @@ public: // If the node was matched and removed, interrupt waiting if (node_removed) { - using ros_middleware_interface::trigger_guard_condition; - trigger_guard_condition(interrupt_guard_condition_); + rmw_trigger_guard_condition(interrupt_guard_condition_); } } @@ -140,8 +143,7 @@ protected: 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. - using ros_middleware_interface::trigger_guard_condition; - trigger_guard_condition(interrupt_guard_condition_); + rmw_trigger_guard_condition(interrupt_guard_condition_); } static void @@ -149,9 +151,8 @@ protected: rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) { std::shared_ptr message = subscription->create_message(); - bool taken = ros_middleware_interface::take( - subscription->subscription_handle_, - message.get()); + auto taken = rmw_take(subscription->subscription_handle_, message.get()); + // TODO(wjwwood): taken is no longer a boolean, check against return types. if (taken) { subscription->handle_message(message); @@ -268,22 +269,22 @@ protected: } // Use the number of subscriptions to allocate memory in the handles size_t number_of_subscriptions = subs.size(); - ros_middleware_interface::SubscriberHandles subscriber_handles; - subscriber_handles.subscriber_count_ = number_of_subscriptions; - // TODO: Avoid redundant malloc's - subscriber_handles.subscribers_ = static_cast( + rmw_subscriptions_t subscriber_handles; + subscriber_handles.subscriber_count = number_of_subscriptions; + // TODO(wjwwood): Avoid redundant malloc's + subscriber_handles.subscribers = static_cast( std::malloc(sizeof(void *) * number_of_subscriptions)); - if (subscriber_handles.subscribers_ == NULL) + if (subscriber_handles.subscribers == NULL) { - // TODO: Use a different error here? maybe std::bad_alloc? + // TODO(wjwwood): Use a different error here? maybe std::bad_alloc? throw std::runtime_error("Could not malloc for subscriber pointers."); } // Then fill the SubscriberHandles with ready subscriptions size_t subscriber_handle_index = 0; for (auto &subscription : subs) { - subscriber_handles.subscribers_[subscriber_handle_index] = \ - subscription->subscription_handle_.data_; + subscriber_handles.subscribers[subscriber_handle_index] = \ + subscription->subscription_handle_->data; subscriber_handle_index += 1; } @@ -333,54 +334,54 @@ protected: // Add 2 to the number for the ctrl-c guard cond and the executor's size_t start_of_timer_guard_conds = 2; size_t number_of_guard_conds = timers.size() + start_of_timer_guard_conds; - ros_middleware_interface::GuardConditionHandles guard_condition_handles; - guard_condition_handles.guard_condition_count_ = number_of_guard_conds; - // TODO: Avoid redundant malloc's - guard_condition_handles.guard_conditions_ = static_cast( + rmw_guard_conditions_t guard_condition_handles; + guard_condition_handles.guard_condition_count = number_of_guard_conds; + // TODO(wjwwood): Avoid redundant malloc's + guard_condition_handles.guard_conditions = static_cast( std::malloc(sizeof(void *) * number_of_guard_conds)); - if (guard_condition_handles.guard_conditions_ == NULL) + if (guard_condition_handles.guard_conditions == NULL) { - // TODO: Use a different error here? maybe std::bad_alloc? + // 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_; + 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_; + guard_condition_handles.guard_conditions[1] = \ + interrupt_guard_condition_->data; // Then fill the SubscriberHandles with ready subscriptions size_t guard_cond_handle_index = start_of_timer_guard_conds; for (auto &timer : timers) { - guard_condition_handles.guard_conditions_[guard_cond_handle_index] = \ - timer->guard_condition_.data_; + guard_condition_handles.guard_conditions[guard_cond_handle_index] = \ + timer->guard_condition_->data; guard_cond_handle_index += 1; } // Now wait on the waitable subscriptions and timers - ros_middleware_interface::wait(subscriber_handles, - guard_condition_handles, - service_handles, - client_handles, - nonblocking); + rmw_wait(&subscriber_handles, + &guard_condition_handles, + &client_handles, + nonblocking); // If ctrl-c guard condition, return directly - if (guard_condition_handles.guard_conditions_[0] != 0) + if (guard_condition_handles.guard_conditions[0] != 0) { // Make sure to free memory - // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed - std::free(subscriber_handles.subscribers_); - std::free(service_handles.services_); - std::free(guard_condition_handles.guard_conditions_); + // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" + // todo has been addressed. + std::free(subscriber_handles.subscribers); + std::free(service_handles.services); + std::free(guard_condition_handles.guard_conditions); return; } // Add the new work to the class's list of things waiting to be executed // Starting with the subscribers for (size_t i = 0; i < number_of_subscriptions; ++i) { - void *handle = subscriber_handles.subscribers_[i]; + void *handle = subscriber_handles.subscribers[i]; if (handle) { subscriber_handles_.push_back(handle); @@ -389,7 +390,7 @@ protected: // Then the timers, start with start_of_timer_guard_conds for (size_t i = start_of_timer_guard_conds; i < number_of_guard_conds; ++i) { - void *handle = guard_condition_handles.guard_conditions_[i]; + void *handle = guard_condition_handles.guard_conditions[i]; if (handle) { guard_condition_handles_.push_back(handle); @@ -415,10 +416,11 @@ protected: } // Make sure to free memory - // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed - std::free(subscriber_handles.subscribers_); - std::free(service_handles.services_); - std::free(guard_condition_handles.guard_conditions_); + // TODO(wjwwood): Remove theses when the "Avoid redundant malloc's" + // todo has been addressed. + std::free(subscriber_handles.subscribers); + std::free(service_handles.services); + std::free(guard_condition_handles.guard_conditions); } /******************************/ @@ -442,7 +444,7 @@ protected: } for (auto subscription : group->subscription_ptrs_) { - if (subscription->subscription_handle_.data_ == subscriber_handle) + if (subscription->subscription_handle_->data == subscriber_handle) { return subscription; } @@ -471,7 +473,7 @@ protected: } for (auto timer : group->timer_ptrs_) { - if (timer->guard_condition_.data_ == guard_condition_handle) + if (timer->guard_condition_->data == guard_condition_handle) { return timer; } @@ -874,9 +876,8 @@ protected: std::shared_ptr get_next_executable(bool nonblocking=false) { - namespace rmi = ros_middleware_interface; // Check to see if there are any subscriptions or timers needing service - // TODO: improve run to run efficiency of this function + // TODO(wjwwood): improve run to run efficiency of this function auto any_exec = get_next_ready_executable(); // If there are none if (!any_exec) @@ -904,7 +905,7 @@ protected: return any_exec; } - ros_middleware_interface::GuardConditionHandle interrupt_guard_condition_; + rmw_guard_condition_t * interrupt_guard_condition_; private: RCLCPP_DISABLE_COPY(Executor); diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp index 908c68d..7f748bc 100644 --- a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -22,8 +22,7 @@ #include #include -#include -#include +#include #include #include diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp index 643fd03..6b9de66 100644 --- a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -21,8 +21,7 @@ #include #include -#include -#include +#include #include #include diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index cbae283..bb54de5 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -29,6 +29,9 @@ #include #include +// Forward declaration of ROS middleware class +namespace rmw {struct rmw_node_t;} + namespace rclcpp { @@ -115,7 +118,7 @@ private: std::string name_; - ros_middleware_interface::NodeHandle node_handle_; + rmw_node_t * node_handle_; rclcpp::context::Context::SharedPtr context_; diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index f08d0bd..45081f4 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -19,11 +19,8 @@ #include #include -#include -#include - -#include -#include +#include +#include #include @@ -44,7 +41,7 @@ Node::Node(std::string node_name, context::Context::SharedPtr context) : name_(node_name), context_(context), number_of_subscriptions_(0), number_of_timers_(0), number_of_services_(0) { - node_handle_ = ::ros_middleware_interface::create_node(); + node_handle_ = rmw_create_node(name_.c_str()); using rclcpp::callback_group::CallbackGroupType; default_callback_group_ = \ create_callback_group(CallbackGroupType::MutuallyExclusive); @@ -65,12 +62,12 @@ template publisher::Publisher::SharedPtr Node::create_publisher(std::string topic_name, size_t queue_size) { - namespace rmi = ::ros_middleware_interface; - - auto type_support_handle = rmi::get_type_support_handle(); - auto publisher_handle = rmi::create_publisher(this->node_handle_, - type_support_handle, - topic_name.c_str()); + using rosidl_generator_cpp::get_type_support_handle; + auto type_support_handle = get_type_support_handle(); + auto publisher_handle = rmw_create_publisher(node_handle_, + type_support_handle, + topic_name.c_str(), + queue_size); return publisher::Publisher::make_shared(publisher_handle); } @@ -98,12 +95,12 @@ Node::create_subscription( std::function &)> callback, rclcpp::callback_group::CallbackGroup::SharedPtr group) { - namespace rmi = ::ros_middleware_interface; - - auto &type_support_handle = rmi::get_type_support_handle(); - auto subscriber_handle = rmi::create_subscriber(this->node_handle_, - type_support_handle, - topic_name.c_str()); + using rosidl_generator_cpp::get_type_support_handle; + auto type_support_handle = get_type_support_handle(); + auto subscriber_handle = rmw_create_subscription(node_handle_, + type_support_handle, + topic_name.c_str(), + queue_size); using namespace rclcpp::subscription; diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index e1133d5..110ec80 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -18,8 +18,7 @@ #include -#include -#include +#include #include @@ -37,18 +36,18 @@ class Publisher public: RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); - Publisher(ros_middleware_interface::PublisherHandle publisher_handle) + Publisher(rmw_publisher_t * publisher_handle) : publisher_handle_(publisher_handle) {} template void publish(std::shared_ptr &msg) { - ::ros_middleware_interface::publish(publisher_handle_, msg.get()); + rmw_publish(publisher_handle_, msg.get()); } private: - ros_middleware_interface::PublisherHandle publisher_handle_; + rmw_publisher_t * publisher_handle_; }; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 3671135..d802faf 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -20,8 +20,7 @@ #include #include -#include -#include +#include #include @@ -41,7 +40,7 @@ public: RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); SubscriptionBase( - ros_middleware_interface::SubscriberHandle subscription_handle, + rmw_subscription_t * subscription_handle, std::string &topic_name) : subscription_handle_(subscription_handle), topic_name_(topic_name) {} @@ -57,7 +56,7 @@ public: private: RCLCPP_DISABLE_COPY(SubscriptionBase); - ros_middleware_interface::SubscriberHandle subscription_handle_; + rmw_subscription_t * subscription_handle_; std::string topic_name_; }; @@ -69,7 +68,7 @@ public: typedef std::function &)> CallbackType; RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); - Subscription(ros_middleware_interface::SubscriberHandle subscription_handle, + Subscription(rmw_subscription_t * subscription_handle, std::string &topic_name, CallbackType callback) : SubscriptionBase(subscription_handle, topic_name), callback_(callback) diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 5167cee..d231c82 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/rclcpp/include/rclcpp/timer.hpp @@ -21,8 +21,7 @@ #include #include -#include -#include +#include #include #include @@ -50,7 +49,7 @@ public: callback_(callback), canceled_(false) { - guard_condition_ = ros_middleware_interface::create_guard_condition(); + guard_condition_ = rmw_create_guard_condition(); } void @@ -64,7 +63,7 @@ public: protected: std::chrono::nanoseconds period_; CallbackType callback_; - ros_middleware_interface::GuardConditionHandle guard_condition_; + rmw_guard_condition_t * guard_condition_; bool canceled_; @@ -98,7 +97,7 @@ public: { return; } - ros_middleware_interface::trigger_guard_condition(guard_condition_); + rmw_trigger_guard_condition(guard_condition_); } } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 57957cc..92cab04 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -16,7 +16,7 @@ #ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ #define RCLCPP_RCLCPP_UTILITIES_HPP_ -// TODO: remove +// TODO(wjwwood): remove #include #include @@ -27,8 +27,7 @@ #include #include -#include -#include +#include // Determine if sigaction is available #if __APPLE__ || _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE @@ -38,8 +37,8 @@ namespace { volatile sig_atomic_t g_signal_status = 0; - ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle = \ - ros_middleware_interface::create_guard_condition(); + rmw_guard_condition_t * g_sigint_guard_cond_handle = \ + rmw_create_guard_condition(); std::condition_variable g_interrupt_condition_variable; std::mutex g_interrupt_mutex; @@ -56,7 +55,7 @@ namespace signal_handler(int signal_value) #endif { - // TODO: remove + // TODO(wjwwood): remove std::cout << "signal_handler(" << signal_value << ")" << std::endl; #ifdef HAS_SIGACTION if (old_action.sa_flags & SA_SIGINFO) @@ -82,8 +81,7 @@ namespace } #endif g_signal_status = signal_value; - using ros_middleware_interface::trigger_guard_condition; - trigger_guard_condition(g_sigint_guard_cond_handle); + rmw_trigger_guard_condition(g_sigint_guard_cond_handle); g_interrupt_condition_variable.notify_all(); } } @@ -99,7 +97,8 @@ namespace utilities void init(int argc, char *argv[]) { - ros_middleware_interface::init(); + // TODO(wjwwood): Handle rmw_ret_t's value. + rmw_init(); #ifdef HAS_SIGACTION struct sigaction action; memset(&action, 0, sizeof(action)); @@ -126,7 +125,7 @@ ok() return ::g_signal_status == 0; } -ros_middleware_interface::GuardConditionHandle +rmw_guard_condition_t * get_global_sigint_guard_condition() { return ::g_sigint_guard_cond_handle;