diff --git a/rclcpp/include/rclcpp/Node.h b/rclcpp/include/rclcpp/Node.h deleted file mode 100644 index 0580680..0000000 --- a/rclcpp/include/rclcpp/Node.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef __rclcpp__Node__h__ -#define __rclcpp__Node__h__ - -#include -#include -#include - -#include "Publisher.h" -#include "Subscriber.h" - -#include "ros_middleware_interface/functions.h" -#include "ros_middleware_interface/handles.h" - -#include "ros_middleware_interface/get_type_support_handle.h" - - -namespace rclcpp -{ - -namespace executor -{ - class SingleThreadExecutor; -} - -class Node -{ - friend class rclcpp::executor::SingleThreadExecutor; -public: - Node() - { - node_handle_ = ::ros_middleware_interface::create_node(); - } - - template - Publisher* create_publisher(const char * topic_name) - { - const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle(); - ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name); - return new Publisher(publisher_handle); - } - - template - Subscriber* create_subscriber(const char * topic_name, typename Subscriber::CallbackType callback) - { - const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle(); - ros_middleware_interface::SubscriberHandle subscriber_handle = ::ros_middleware_interface::create_subscriber(node_handle_, type_support_handle, topic_name); - SubscriberInterface *sub = new Subscriber(subscriber_handle, std::string(topic_name), callback); - this->subscribers_.push_back(sub); - return static_cast *>(sub); - } - -private: - ros_middleware_interface::NodeHandle node_handle_; - std::vector subscribers_; - -}; - -rclcpp::Node* create_node() -{ - return new rclcpp::Node(); -} - -} /* namespace rclcpp */ - - -#endif // __rclcpp__Node__h__ diff --git a/rclcpp/include/rclcpp/Publisher.h b/rclcpp/include/rclcpp/Publisher.h deleted file mode 100644 index bea0573..0000000 --- a/rclcpp/include/rclcpp/Publisher.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef __rclcpp__Publisher__h__ -#define __rclcpp__Publisher__h__ - -#include "ros_middleware_interface/functions.h" -#include "ros_middleware_interface/handles.h" - - -namespace rclcpp -{ - -template -class Publisher -{ -public: - Publisher(const ros_middleware_interface::PublisherHandle& publisher_handle) - : publisher_handle_(publisher_handle) - {} - - void publish(const ROSMessage& ros_message) - { - ::ros_middleware_interface::publish(publisher_handle_, &ros_message); - } - -private: - ros_middleware_interface::PublisherHandle publisher_handle_; -}; - -} - -#endif // __rclcpp__Publisher__h__ diff --git a/rclcpp/include/rclcpp/Subscriber.h b/rclcpp/include/rclcpp/Subscriber.h deleted file mode 100644 index 8d73559..0000000 --- a/rclcpp/include/rclcpp/Subscriber.h +++ /dev/null @@ -1,75 +0,0 @@ -#ifndef __rclcpp__Subscriber__h__ -#define __rclcpp__Subscriber__h__ - -#include -#include - -#include "ros_middleware_interface/functions.h" -#include "ros_middleware_interface/handles.h" - - -namespace rclcpp -{ - -class Node; - -namespace executor -{ - class SingleThreadExecutor; -} - -class SubscriberInterface -{ - friend class rclcpp::executor::SingleThreadExecutor; -public: - SubscriberInterface(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name) - : subscriber_handle_(subscriber_handle), topic_name_(topic_name) - {} -private: - virtual void * create_message() = 0; - - virtual void delete_message(void * ros_message) = 0; - - virtual void handle_message(void * ros_message) = 0; - - ros_middleware_interface::SubscriberHandle subscriber_handle_; - std::string topic_name_; - -}; - -template -class Subscriber : public SubscriberInterface -{ - friend class rclcpp::Node; -public: - typedef std::function CallbackType; - Subscriber(const ros_middleware_interface::SubscriberHandle &subscriber_handle, std::string topic_name, CallbackType callback) - : SubscriberInterface(subscriber_handle, topic_name), callback_(callback) - {} - -private: - void * create_message() - { - return new ROSMessage(); - } - - void delete_message(void * ros_message) - { - ROSMessage* msg = (ROSMessage*)ros_message; - delete msg; - ros_message = 0; - } - - void handle_message(void * ros_message) - { - ROSMessage* msg = (ROSMessage*)ros_message; - callback_(msg); - } - - CallbackType callback_; - -}; - -} - -#endif // __rclcpp__Subscriber__h__ diff --git a/rclcpp/include/rclcpp/callback_group.hpp b/rclcpp/include/rclcpp/callback_group.hpp new file mode 100644 index 0000000..bfe4431 --- /dev/null +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -0,0 +1,75 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ +#define RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ + +#include +#include +#include + +#include +#include + +namespace rclcpp +{ + +// Forward declarations for friend statement in class CallbackGroup +namespace node {class Node;} +namespace executor {class Executor;} + +namespace callback_group +{ + +enum class CallbackGroupType {MutuallyExclusive, Reentrant}; + +class CallbackGroup +{ + friend class rclcpp::node::Node; + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup); + + CallbackGroup(CallbackGroupType group_type) + : type_(group_type), can_be_taken_from_(true) + {} + +private: + RCLCPP_DISABLE_COPY(CallbackGroup); + + void + add_subscription( + const subscription::SubscriptionBase::SharedPtr &subscription_ptr) + { + subscription_ptrs_.push_back(subscription_ptr); + } + + void + add_timer(const timer::TimerBase::SharedPtr &timer_ptr) + { + timer_ptrs_.push_back(timer_ptr); + } + + CallbackGroupType type_; + std::vector subscription_ptrs_; + std::vector timer_ptrs_; + std::atomic_bool can_be_taken_from_; + +}; + +} /* namespace callback_group */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */ diff --git a/rclcpp/include/rclcpp/context.hpp b/rclcpp/include/rclcpp/context.hpp new file mode 100644 index 0000000..ab4db0e --- /dev/null +++ b/rclcpp/include/rclcpp/context.hpp @@ -0,0 +1,44 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_CONTEXT_HPP_ +#define RCLCPP_RCLCPP_CONTEXT_HPP_ + +#include + +#include + +namespace rclcpp +{ +namespace context +{ + +/* ROS Context */ +class Context +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(Context); + + Context() {} + +private: + RCLCPP_DISABLE_COPY(Context); + +}; + +} /* namespace context */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_CONTEXT_HPP_ */ diff --git a/rclcpp/include/rclcpp/contexts/default_context.hpp b/rclcpp/include/rclcpp/contexts/default_context.hpp new file mode 100644 index 0000000..8ba75d6 --- /dev/null +++ b/rclcpp/include/rclcpp/contexts/default_context.hpp @@ -0,0 +1,41 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ +#define RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ + +#include + +namespace rclcpp +{ +namespace contexts +{ +namespace default_context +{ + +class DefaultContext : public rclcpp::context::Context +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(DefaultContext); + + DefaultContext() {} + +}; + +} +} +} + +#endif /* RCLCPP_RCLCPP_CONTEXTS_DEFAULT_CONTEXT_HPP_ */ diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp new file mode 100644 index 0000000..40f4d1d --- /dev/null +++ b/rclcpp/include/rclcpp/executor.hpp @@ -0,0 +1,589 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_EXECUTOR_HPP_ +#define RCLCPP_RCLCPP_EXECUTOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace rclcpp +{ +namespace executor +{ + +class Executor +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(Executor); + + Executor() + : interrupt_guard_condition_( + ros_middleware_interface::create_guard_condition()) + {} + virtual ~Executor() {} + + virtual void spin() = 0; + + virtual void + add_node(rclcpp::node::Node::SharedPtr &node_ptr) + { + // Check to ensure node not already added + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (node == node_ptr) + { + // TODO: Use a different error here? + throw std::runtime_error( + "Cannot add node to executor, node already added."); + } + } + 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_); + } + + virtual void + remove_node(rclcpp::node::Node::SharedPtr &node_ptr) + { + 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 the node was matched and removed, interrupt waiting + if (node_removed) + { + using ros_middleware_interface::trigger_guard_condition; + trigger_guard_condition(interrupt_guard_condition_); + } + } + + void spin_node_some(rclcpp::node::Node::SharedPtr &node) + { + this->add_node(node); + // non-blocking = true + std::shared_ptr any_exec; + while ((any_exec = get_next_executable(true))) + { + execute_any_executable(any_exec); + } + this->remove_node(node); + } + +protected: + struct AnyExecutable + { + AnyExecutable() : subscription(0), timer(0), callback_group(0), node(0) {} + // Either the subscription or the timer will be set, but not both + rclcpp::subscription::SubscriptionBase::SharedPtr subscription; + rclcpp::timer::TimerBase::SharedPtr timer; + // These are used to keep the scope on the containing items + rclcpp::callback_group::CallbackGroup::SharedPtr callback_group; + rclcpp::node::Node::SharedPtr node; + }; + + void + execute_any_executable(std::shared_ptr &any_exec) + { + if (!any_exec) + { + return; + } + if (any_exec->timer) + { + execute_timer(any_exec->timer); + } + if (any_exec->subscription) + { + execute_subscription(any_exec->subscription); + } + // 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. + using ros_middleware_interface::trigger_guard_condition; + trigger_guard_condition(interrupt_guard_condition_); + } + + static void + execute_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) + { + std::shared_ptr message = subscription->create_message(); + bool taken = ros_middleware_interface::take( + subscription->subscription_handle_, + message.get()); + if (taken) + { + subscription->handle_message(message); + } + else + { + std::cout << "[rclcpp::error] take failed for subscription on topic: " + << subscription->get_topic_name() + << std::endl; + } + } + + static void + execute_timer( + rclcpp::timer::TimerBase::SharedPtr &timer) + { + timer->callback_(); + } + +/*** Populate class storage from stored weak node pointers and wait. ***/ + + void + wait_for_work(bool nonblocking) + { + // Collect the subscriptions and timers to be waited on + bool has_invalid_weak_nodes = false; + std::vector subs; + std::vector timers; + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + has_invalid_weak_nodes = false; + continue; + } + for (auto &weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + if (!group || group->can_be_taken_from_.load() == false) + { + continue; + } + for (auto &subscription : group->subscription_ptrs_) + { + subs.push_back(subscription); + } + for (auto &timer : group->timer_ptrs_) + { + timers.push_back(timer); + } + } + } + // 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 + 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( + std::malloc(sizeof(void *) * number_of_subscriptions)); + if (subscriber_handles.subscribers_ == NULL) + { + // TODO: 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_handle_index += 1; + } + // Use the number of guard conditions to allocate memory in the handles + // Add 2 to the number for the ctrl-c guard cond and the executor's + size_t start_of_timer_guard_conds = 2; + 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( + std::malloc(sizeof(void *) * number_of_guard_conds)); + if (guard_condition_handles.guard_conditions_ == NULL) + { + // TODO: 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_; + // 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_cond_handle_index += 1; + } + // Now wait on the waitable subscriptions and timers + ros_middleware_interface::wait(subscriber_handles, + guard_condition_handles, + nonblocking); + // If ctrl-c guard condition, return directly + 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(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]; + if (handle) + { + subscriber_handles_.push_back(handle); + } + } + // 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]; + if (handle) + { + guard_condition_handles_.push_back(handle); + } + } + // Make sure to free memory + // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed + std::free(subscriber_handles.subscribers_); + std::free(guard_condition_handles.guard_conditions_); + } + +/******************************/ + + rclcpp::subscription::SubscriptionBase::SharedPtr + get_subscription_by_handle(void * subscriber_handle) + { + for (auto weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + if (!group) + { + continue; + } + for (auto subscription : group->subscription_ptrs_) + { + if (subscription->subscription_handle_.data_ == subscriber_handle) + { + return subscription; + } + } + } + } + return rclcpp::subscription::SubscriptionBase::SharedPtr(); + } + + rclcpp::timer::TimerBase::SharedPtr + get_timer_by_handle(void * guard_condition_handle) + { + for (auto weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + if (!group) + { + continue; + } + for (auto timer : group->timer_ptrs_) + { + if (timer->guard_condition_.data_ == guard_condition_handle) + { + return timer; + } + } + } + } + return rclcpp::timer::TimerBase::SharedPtr(); + } + + void + remove_subscriber_handle_from_subscriber_handles(void *handle) + { + subscriber_handles_.remove(handle); + } + + void + remove_guard_condition_handle_from_guard_condition_handles(void *handle) + { + guard_condition_handles_.remove(handle); + } + + 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->callback_groups_) + { + auto callback_group = weak_group.lock(); + if (!callback_group) + { + continue; + } + if (callback_group == group) + { + return node; + } + } + } + return rclcpp::node::Node::SharedPtr(); + } + + rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_timer( + rclcpp::timer::TimerBase::SharedPtr &timer) + { + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto &weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + for (auto &t : group->timer_ptrs_) + { + if (t == timer) + { + return group; + } + } + } + } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); + } + + void + get_next_timer(std::shared_ptr &any_exec) + { + for (auto handle : guard_condition_handles_) + { + auto timer = get_timer_by_handle(handle); + if (timer) + { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_timer(timer); + if (!group) + { + // Group was not found, meaning the timer is not valid... + // Remove it from the ready list and continue looking + remove_guard_condition_handle_from_guard_condition_handles(handle); + continue; + } + if (!group->can_be_taken_from_.load()) + { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec->timer = timer; + any_exec->callback_group = group; + any_exec->node = get_node_by_group(group); + remove_guard_condition_handle_from_guard_condition_handles(handle); + return; + } + // Else, the timer is no longer valid, remove it and continue + remove_guard_condition_handle_from_guard_condition_handles(handle); + } + } + + rclcpp::callback_group::CallbackGroup::SharedPtr + get_group_by_subscription( + rclcpp::subscription::SubscriptionBase::SharedPtr &subscription) + { + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + continue; + } + for (auto &weak_group : node->callback_groups_) + { + auto group = weak_group.lock(); + for (auto &sub : group->subscription_ptrs_) + { + if (sub == subscription) + { + return group; + } + } + } + } + return rclcpp::callback_group::CallbackGroup::SharedPtr(); + } + + void + get_next_subscription(std::shared_ptr &any_exec) + { + for (auto handle : subscriber_handles_) + { + auto subscription = get_subscription_by_handle(handle); + if (subscription) + { + // Find the group for this handle and see if it can be serviced + auto group = get_group_by_subscription(subscription); + if (!group) + { + // Group was not found, meaning the subscription is not valid... + // Remove it from the ready list and continue looking + remove_subscriber_handle_from_subscriber_handles(handle); + continue; + } + if (!group->can_be_taken_from_.load()) + { + // Group is mutually exclusive and is being used, so skip it for now + // Leave it to be checked next time, but continue searching + continue; + } + // Otherwise it is safe to set and return the any_exec + any_exec->subscription = subscription; + any_exec->callback_group = group; + any_exec->node = get_node_by_group(group); + remove_subscriber_handle_from_subscriber_handles(handle); + return; + } + // Else, the subscription is no longer valid, remove it and continue + remove_subscriber_handle_from_subscriber_handles(handle); + } + } + + std::shared_ptr + get_next_ready_executable() + { + std::shared_ptr any_exec(new AnyExecutable()); + // 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 + get_next_subscription(any_exec); + if (any_exec->subscription) + { + return any_exec; + } + // If there is neither a ready timer nor subscription, return a null ptr + any_exec.reset(); + return any_exec; + } + + 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 + 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(nonblocking); + // 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->type_ == \ + callback_group::CallbackGroupType::MutuallyExclusive) + { + // It should not have been taken otherwise + assert(any_exec->callback_group->can_be_taken_from_.load() == true); + // 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; + } + + ros_middleware_interface::GuardConditionHandle interrupt_guard_condition_; + +private: + RCLCPP_DISABLE_COPY(Executor); + + std::vector> weak_nodes_; + typedef std::list SubscriberHandles; + SubscriberHandles subscriber_handles_; + typedef std::list GuardConditionHandles; + GuardConditionHandles guard_condition_handles_; + +}; + +} /* executor */ +} /* rclcpp */ + +#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */ diff --git a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h b/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h deleted file mode 100644 index d44b182..0000000 --- a/rclcpp/include/rclcpp/executor/SingleThreadExecutor.h +++ /dev/null @@ -1,108 +0,0 @@ -#ifndef __rclcpp__executor__SingleThreadExecutor__h__ -#define __rclcpp__executor__SingleThreadExecutor__h__ - -#include -#include -#include -#include - -#include "rclcpp/Node.h" -#include "ros_middleware_interface/functions.h" -#include "ros_middleware_interface/handles.h" - -namespace rclcpp -{ - -namespace executor -{ - -class SingleThreadExecutor -{ -public: - SingleThreadExecutor() {} - - void register_node(rclcpp::Node *node) - { - this->nodes_.push_back(node); - } - - void unregister_node(rclcpp::Node *node) - { - auto it = std::find(this->nodes_.begin(), this->nodes_.end(), node); - if (it != this->nodes_.end()) - { - this->nodes_.erase(it); - } - } - - void exec() - { - while (1) - { - size_t total_subscribers_size = 0; - for (auto node : this->nodes_) - { - total_subscribers_size += node->subscribers_.size(); - } - - ros_middleware_interface::SubscriberHandles subscriber_handles; - subscriber_handles.subscriber_count_ = total_subscribers_size; - subscriber_handles.subscribers_ = static_cast(malloc(sizeof(void *) * total_subscribers_size)); - - size_t handles_index = 0; - for (auto node : this->nodes_) { - for (auto subscriber : node->subscribers_) - { - assert(handles_index < total_subscribers_size); - subscriber_handles.subscribers_[handles_index] = subscriber->subscriber_handle_.data_; - handles_index += 1; - } - } - - ros_middleware_interface::GuardConditionHandles guard_condition_handles; - guard_condition_handles.guard_condition_count_ = 0; - guard_condition_handles.guard_conditions_ = 0; - - ros_middleware_interface::wait(subscriber_handles, guard_condition_handles, false); - - for (size_t index = 0; index < total_subscribers_size; ++index) - { - void *handle = subscriber_handles.subscribers_[index]; - if (!handle) - { - continue; - } - for (auto node : this->nodes_) - { - for (auto subscriber : node->subscribers_) - { - if (subscriber->subscriber_handle_.data_ == handle) - { - // Do callback - std::cout << "Callback for subscriber of topic: " << subscriber->topic_name_ << std::endl; - void * ros_msg = subscriber->create_message(); - bool taken = ros_middleware_interface::take(subscriber->subscriber_handle_, ros_msg); - if (taken) - { - std::cout << "- received message on topic: " << subscriber->topic_name_ << std::endl; - subscriber->handle_message(ros_msg); - } - subscriber->delete_message(ros_msg); - } - } - } - } - } - } - -private: - SingleThreadExecutor(const SingleThreadExecutor&); - std::vector nodes_; - -}; - -} /* namespace executor */ - -} /* namespace rclcpp */ - -#endif // __rclcpp__executor__SingleThreadExecutor__h__ diff --git a/rclcpp/include/rclcpp/executors.hpp b/rclcpp/include/rclcpp/executors.hpp new file mode 100644 index 0000000..dfceb03 --- /dev/null +++ b/rclcpp/include/rclcpp/executors.hpp @@ -0,0 +1,33 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_EXECUTORS_HPP_ +#define RCLCPP_RCLCPP_EXECUTORS_HPP_ + +#include +#include + +namespace rclcpp +{ +namespace executors +{ + +using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor; +using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor; + +} +} + +#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */ diff --git a/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp new file mode 100644 index 0000000..908c68d --- /dev/null +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -0,0 +1,112 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ +#define RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ +namespace multi_threaded_executor +{ + +class MultiThreadedExecutor : public executor::Executor +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(MultiThreadedExecutor); + + MultiThreadedExecutor() + { + number_of_threads_ = std::thread::hardware_concurrency(); + if (number_of_threads_ == 0) + { + number_of_threads_ = 1; + } + } + + ~MultiThreadedExecutor() {} + + void + 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 + get_number_of_threads() + { + return number_of_threads_; + } + +private: + void run(size_t this_thread_id) + { + rclcpp::thread_id = this_thread_id; + while (rclcpp::utilities::ok()) + { + std::shared_ptr any_exec; + { + std::lock_guard wait_lock(wait_mutex_); + if (!rclcpp::utilities::ok()) + { + return; + } + any_exec = get_next_executable(); + } + execute_any_executable(any_exec); + } + } + + RCLCPP_DISABLE_COPY(MultiThreadedExecutor); + + std::mutex wait_mutex_; + size_t number_of_threads_; + +}; + +} /* namespace multi_threaded_executor */ +} /* namespace executors */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_EXECUTORS_MULTI_THREADED_EXECUTOR_HPP_ */ diff --git a/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp new file mode 100644 index 0000000..643fd03 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -0,0 +1,67 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ +#define RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace rclcpp +{ +namespace executors +{ +namespace single_threaded_executor +{ + +class SingleThreadedExecutor : public executor::Executor +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(SingleThreadedExecutor); + + SingleThreadedExecutor() {} + + ~SingleThreadedExecutor() {} + + void spin() + { + while (rclcpp::utilities::ok()) + { + auto any_exec = get_next_executable(); + execute_any_executable(any_exec); + } + } + +private: + RCLCPP_DISABLE_COPY(SingleThreadedExecutor); + +}; + +} /* namespace single_threaded_executor */ +} /* namespace executors */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_EXECUTORS_SINGLE_THREADED_EXECUTOR_HPP_ */ diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp new file mode 100644 index 0000000..ba59845 --- /dev/null +++ b/rclcpp/include/rclcpp/macros.hpp @@ -0,0 +1,44 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_MACROS_HPP_ +#define RCLCPP_RCLCPP_MACROS_HPP_ + +/* Disables the copy constructor and operator= for the given class. + * + * Use in the private section of the class. + */ +#define RCLCPP_DISABLE_COPY(Class) \ + Class(const Class&) = delete; \ + Class& operator=(const Class&) = delete; + +/* Defines a make_shared static function on the class using perfect forwarding. + * + * Use in the public section of the class. + * Make sure to include in the header when using this. + */ +#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \ + typedef std::shared_ptr SharedPtr; \ + \ + template \ + static std::shared_ptr \ + make_shared(Args &&...args) \ + { \ + return std::make_shared(std::forward(args)...); \ + } + +#define RCLCPP_INFO(Args) std::cout << Args << std::endl; + +#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */ diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp new file mode 100644 index 0000000..e19f338 --- /dev/null +++ b/rclcpp/include/rclcpp/node.hpp @@ -0,0 +1,120 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_NODE_HPP_ +#define RCLCPP_RCLCPP_NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace rclcpp +{ + +// Forward declaration for friend statement +namespace executor {class Executor;} + +namespace node +{ + +/* ROS Node Interface. + * + * This is the single point of entry for creating publishers and subscribers. + */ +class Node +{ + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(Node); + + /* Create a node based on the node name. */ + Node(std::string node_name); + /* Create a node based on the node name and a rclcpp::context::Context. */ + Node(std::string node_name, rclcpp::context::Context::SharedPtr context); + + /* Get the name of the node. */ + std::string + get_name(); + + /* Create and return a callback group. */ + rclcpp::callback_group::CallbackGroup::SharedPtr + create_callback_group(rclcpp::callback_group::CallbackGroupType group_type); + + /* Create and return a Publisher. */ + template rclcpp::publisher::Publisher::SharedPtr + create_publisher(std::string topic_name, size_t queue_size); + + /* Create and return a Subscription. */ + template + typename rclcpp::subscription::Subscription::SharedPtr + create_subscription( + std::string topic_name, + size_t queue_size, + std::function &)> callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + + /* Create a timer. */ + rclcpp::timer::WallTimer::SharedPtr + create_wall_timer( + std::chrono::nanoseconds period, + rclcpp::timer::CallbackType callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + + rclcpp::timer::WallTimer::SharedPtr + create_wall_timer( + std::chrono::duration period, + rclcpp::timer::CallbackType callback, + rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr); + + typedef rclcpp::callback_group::CallbackGroup CallbackGroup; + typedef std::weak_ptr CallbackGroupWeakPtr; + typedef std::list CallbackGroupWeakPtrList; + +private: + RCLCPP_DISABLE_COPY(Node); + + bool + group_in_node(callback_group::CallbackGroup::SharedPtr &group); + + std::string name_; + + ros_middleware_interface::NodeHandle node_handle_; + + rclcpp::context::Context::SharedPtr context_; + + CallbackGroup::SharedPtr default_callback_group_; + CallbackGroupWeakPtrList callback_groups_; + + size_t number_of_subscriptions_; + size_t number_of_timers_; + +}; + +} /* namespace node */ +} /* namespace rclcpp */ + +#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_ +// Template implementations +#include "node_impl.hpp" +#endif + +#endif /* RCLCPP_RCLCPP_NODE_HPP_ */ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp new file mode 100644 index 0000000..22856df --- /dev/null +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -0,0 +1,165 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_NODE_IMPL_HPP_ +#define RCLCPP_RCLCPP_NODE_IMPL_HPP_ + +#include +#include + +#include +#include + +#include + +#include + +#ifndef RCLCPP_RCLCPP_NODE_HPP_ +#include "node.hpp" +#endif + +using namespace rclcpp; +using namespace rclcpp::node; + +using rclcpp::contexts::default_context::DefaultContext; + +Node::Node(std::string node_name) + : Node(node_name, DefaultContext::make_shared()) +{} + +Node::Node(std::string node_name, context::Context::SharedPtr context) + : name_(node_name), context_(context), + number_of_subscriptions_(0), number_of_timers_(0) +{ + node_handle_ = ::ros_middleware_interface::create_node(); + using rclcpp::callback_group::CallbackGroupType; + default_callback_group_ = \ + create_callback_group(CallbackGroupType::MutuallyExclusive); +} + +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; +} + +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()); + + return publisher::Publisher::make_shared(publisher_handle); +} + +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 subscription::Subscription::SharedPtr +Node::create_subscription( + std::string topic_name, + size_t queue_size, + 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 namespace rclcpp::subscription; + + auto sub = Subscription::make_shared(subscriber_handle, + topic_name, + callback); + auto sub_base_ptr = std::dynamic_pointer_cast(sub); + if (group) + { + if (!group_in_node(group)) + { + // TODO: use custom exception + throw std::runtime_error("Cannot create timer, group not in node."); + } + group->add_subscription(sub_base_ptr); + } + else + { + default_callback_group_->add_subscription(sub_base_ptr); + } + number_of_subscriptions_++; + return sub; +} + +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: 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; +} + +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); +} + +#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */ diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp new file mode 100644 index 0000000..e1133d5 --- /dev/null +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -0,0 +1,58 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_PUBLISHER_HPP_ +#define RCLCPP_RCLCPP_PUBLISHER_HPP_ + +#include + +#include +#include + +#include + +namespace rclcpp +{ + +// Forward declaration for friend statement +namespace node {class Node;} + +namespace publisher +{ + +class Publisher +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(Publisher); + + Publisher(ros_middleware_interface::PublisherHandle publisher_handle) + : publisher_handle_(publisher_handle) + {} + + template void + publish(std::shared_ptr &msg) + { + ::ros_middleware_interface::publish(publisher_handle_, msg.get()); + } + +private: + ros_middleware_interface::PublisherHandle publisher_handle_; + +}; + +} /* namespace publisher */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_PUBLISHER_HPP_ */ diff --git a/rclcpp/include/rclcpp/rate.hpp b/rclcpp/include/rclcpp/rate.hpp new file mode 100644 index 0000000..4fe225e --- /dev/null +++ b/rclcpp/include/rclcpp/rate.hpp @@ -0,0 +1,120 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_RATE_HPP_ +#define RCLCPP_RCLCPP_RATE_HPP_ + +#include +#include +#include + +#include +#include + +namespace rclcpp +{ +namespace rate +{ + +class RateBase +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(RateBase); + + virtual bool sleep() = 0; + virtual bool is_steady() = 0; + virtual void reset() = 0; +}; + +using std::chrono::duration; +using std::chrono::duration_cast; +using std::chrono::nanoseconds; + +template +class GenericRate : public RateBase +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate); + + GenericRate(double rate) + : GenericRate( + duration_cast(duration(1.0/rate))) + {} + GenericRate(std::chrono::nanoseconds period) + : period_(period), last_interval_(Clock::now()) + {} + + virtual bool + sleep() + { + // Time coming into sleep + auto now = Clock::now(); + // Time of next interval + auto next_interval = last_interval_ + period_; + // Detect backwards time flow + if (now < last_interval_) + { + // Best thing to do is to set the next_interval to now + period + next_interval = now + period_; + } + // Calculate the time to sleep + auto time_to_sleep = next_interval - now; + // Update the interval + last_interval_ += period_; + // If the time_to_sleep is negative or zero, don't sleep + if (time_to_sleep <= std::chrono::seconds(0)) + { + // If an entire cycle was missed then reset next interval. + // This might happen if the loop took more than a cycle. + // Or if time jumps forward. + if (now > next_interval + period_) + { + last_interval_ = now + period_; + } + // Either way do not sleep and return false + return false; + } + // Sleep (will get interrupted by ctrl-c, may not sleep full time) + rclcpp::utilities::sleep_for(time_to_sleep); + return true; + } + + virtual bool + is_steady() + { + return Clock::is_steady; + } + + virtual void + reset() + { + last_interval_ = Clock::now(); + } + +private: + RCLCPP_DISABLE_COPY(GenericRate); + + std::chrono::nanoseconds period_; + std::chrono::time_point last_interval_; + +}; + +typedef GenericRate Rate; +typedef GenericRate WallRate; + +} +} + +#endif /* RCLCPP_RCLCPP_RATE_HPP_ */ diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp new file mode 100644 index 0000000..97b41bf --- /dev/null +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -0,0 +1,75 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_RCLCPP_HPP_ +#define RCLCPP_RCLCPP_RCLCPP_HPP_ + +#include +#include + +#include +#include +#include +#include + +namespace rclcpp +{ + +constexpr std::chrono::seconds operator "" _s(unsigned long long s) +{ + return std::chrono::seconds(s); +} +constexpr std::chrono::duration operator "" _s(long double s) +{ + return std::chrono::duration(s); +} + +constexpr std::chrono::nanoseconds +operator "" _ns(unsigned long long ns) +{ + return std::chrono::nanoseconds(ns); +} +constexpr std::chrono::duration +operator "" _ns(long double ns) +{ + return std::chrono::duration(ns); +} + +using rclcpp::node::Node; +using rclcpp::publisher::Publisher; +using rclcpp::subscription::SubscriptionBase; +using rclcpp::subscription::Subscription; +using rclcpp::rate::GenericRate; +using rclcpp::rate::WallRate; +using rclcpp::utilities::ok; +using rclcpp::utilities::init; +using rclcpp::utilities::sleep_for; + +void spin_some(Node::SharedPtr &node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.spin_node_some(node_ptr); +} + +void spin(Node::SharedPtr &node_ptr) +{ + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_ptr); + executor.spin(); +} + +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_RCLCPP_HPP_ */ diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp new file mode 100644 index 0000000..3671135 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -0,0 +1,99 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ +#define RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ + +#include +#include +#include + +#include +#include + +#include + +namespace rclcpp +{ + +// Forward declaration is for friend statement in SubscriptionBase +namespace executor {class Executor;} + +namespace subscription +{ + +class SubscriptionBase +{ + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(SubscriptionBase); + + SubscriptionBase( + ros_middleware_interface::SubscriberHandle subscription_handle, + std::string &topic_name) + : subscription_handle_(subscription_handle), topic_name_(topic_name) + {} + + std::string get_topic_name() + { + return this->topic_name_; + } + + virtual std::shared_ptr create_message() = 0; + virtual void handle_message(std::shared_ptr &message) = 0; + +private: + RCLCPP_DISABLE_COPY(SubscriptionBase); + + ros_middleware_interface::SubscriberHandle subscription_handle_; + std::string topic_name_; + +}; + +template +class Subscription : public SubscriptionBase +{ +public: + typedef std::function &)> CallbackType; + RCLCPP_MAKE_SHARED_DEFINITIONS(Subscription); + + Subscription(ros_middleware_interface::SubscriberHandle subscription_handle, + std::string &topic_name, + CallbackType callback) + : SubscriptionBase(subscription_handle, topic_name), callback_(callback) + {} + + std::shared_ptr create_message() + { + return std::shared_ptr(new MessageT()); + } + + void handle_message(std::shared_ptr &message) + { + auto typed_message = std::static_pointer_cast(message); + callback_(typed_message); + } + +private: + RCLCPP_DISABLE_COPY(Subscription); + + CallbackType callback_; + +}; + +} /* namespace subscription */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */ diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp new file mode 100644 index 0000000..5167cee --- /dev/null +++ b/rclcpp/include/rclcpp/timer.hpp @@ -0,0 +1,124 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_TIMER_HPP_ +#define RCLCPP_RCLCPP_TIMER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace rclcpp +{ + +// Forward declaration is for friend statement in GenericTimer +namespace executor {class Executor;} + +namespace timer +{ + +typedef std::function CallbackType; + +class TimerBase +{ + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(TimerBase); + + TimerBase(std::chrono::nanoseconds period, CallbackType callback) + : period_(period), + callback_(callback), + canceled_(false) + { + guard_condition_ = ros_middleware_interface::create_guard_condition(); + } + + void + cancel() + { + this->canceled_ = true; + } + + virtual bool is_steady() = 0; + +protected: + std::chrono::nanoseconds period_; + CallbackType callback_; + ros_middleware_interface::GuardConditionHandle guard_condition_; + + bool canceled_; + +}; + +template +class GenericTimer : public TimerBase +{ + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(GenericTimer); + + GenericTimer(std::chrono::nanoseconds period, CallbackType callback) + : TimerBase(period, callback), loop_rate_(period) + { + thread_ = std::thread(&GenericTimer::run, this); + } + + ~GenericTimer() + { + cancel(); + } + + void + run() + { + while (rclcpp::utilities::ok() && !this->canceled_) + { + loop_rate_.sleep(); + if (!rclcpp::utilities::ok()) + { + return; + } + ros_middleware_interface::trigger_guard_condition(guard_condition_); + } + } + + virtual bool + is_steady() + { + return Clock::is_steady; + } + +private: + RCLCPP_DISABLE_COPY(GenericTimer); + + std::thread thread_; + rclcpp::rate::GenericRate loop_rate_; + +}; + +typedef GenericTimer WallTimer; + +} /* namespace timer */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */ diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp new file mode 100644 index 0000000..5555346 --- /dev/null +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -0,0 +1,152 @@ +/* Copyright 2014 Open Source Robotics Foundation, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef RCLCPP_RCLCPP_UTILITIES_HPP_ +#define RCLCPP_RCLCPP_UTILITIES_HPP_ + +// TODO: remove +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +// Determine if sigaction is available +#if _POSIX_C_SOURCE >= 1 || _XOPEN_SOURCE || _POSIX_SOURCE +#define HAS_SIGACTION +#endif + +namespace +{ + volatile sig_atomic_t g_signal_status = 0; + ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle = \ + ros_middleware_interface::create_guard_condition(); + std::condition_variable g_interrupt_condition_variable; + std::mutex g_interrupt_mutex; + +#ifdef HAS_SIGACTION + struct sigaction old_action; +#else + 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: 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 + { + if (old_action.sa_handler != NULL && // Is set + old_action.sa_handler != SIG_DFL && // Is not default + old_action.sa_handler != SIG_IGN) // Is not ignored + { + old_action.sa_handler(signal_value); + } + } +#else + if (old_signal_handler) + { + old_signal_handler(signal_value); + } +#endif + g_signal_status = signal_value; + using ros_middleware_interface::trigger_guard_condition; + trigger_guard_condition(g_sigint_guard_cond_handle); + g_interrupt_condition_variable.notify_all(); + } +} + +namespace rclcpp +{ + +__thread size_t thread_id = 0; + +namespace utilities +{ + +void +init(int argc, char *argv[]) +{ + ros_middleware_interface::init(); +#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 + { + throw std::runtime_error( + std::string("Failed to set SIGINT signal handler: (" + + std::to_string(errno) + ")") + + std::strerror(errno)); + } +} + +bool +ok() +{ + return ::g_signal_status == 0; +} + +ros_middleware_interface::GuardConditionHandle +get_global_sigint_guard_condition() +{ + return ::g_sigint_guard_cond_handle; +} + +template +bool +sleep_for(const std::chrono::duration& sleep_duration) +{ + // TODO: determine if posix's nanosleep(2) is more efficient here + std::unique_lock lock(::g_interrupt_mutex); + auto cvs = ::g_interrupt_condition_variable.wait_for(lock, sleep_duration); + return cvs == std::cv_status::no_timeout; +} + +} /* namespace utilities */ +} /* namespace rclcpp */ + +#ifdef HAS_SIGACTION +#undef HAS_SIGACTION +#endif + +#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */