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..483eb62 --- /dev/null +++ b/rclcpp/include/rclcpp/callback_group.hpp @@ -0,0 +1,64 @@ +/* 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 + +namespace rclcpp +{ + +// Forward declarations for friend statement in class CallbackGroup +namespace node {class Node;} +namespace executor {class Executor;} + +namespace callback_group +{ + +enum class CallbackGroupType {NonThreadSafe, ThreadSafe}; + +class CallbackGroup +{ + friend class rclcpp::node::Node; + friend class rclcpp::executor::Executor; +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(CallbackGroup); + +private: + CallbackGroup(std::string group_name, CallbackGroupType group_type) + : name_(group_name), type_(group_type) + {} + RCLCPP_DISABLE_COPY(CallbackGroup); + + void + add_subscription(subscription::SubscriptionBase::SharedPtr &subscription_ptr) + { + subscription_ptrs_.push_back(subscription_ptr); + } + + std::string name_; + CallbackGroupType type_; + std::vector subscription_ptrs_; + +}; + +} /* 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..11b91b3 --- /dev/null +++ b/rclcpp/include/rclcpp/executor.hpp @@ -0,0 +1,293 @@ +/* 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() + { + subscriber_handles_.subscriber_count_ = 0; + subscriber_handles_.subscribers_ = 0; + } + ~Executor() {} + + void add_node(rclcpp::node::Node::SharedPtr &node_ptr) + { + this->weak_nodes_.push_back(node_ptr); + } + +protected: + 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; + } + } + + void reset_subscriber_handles() + { + if (subscriber_handles_.subscriber_count_ != 0) + { + subscriber_handles_.subscriber_count_ = 0; + std::free(subscriber_handles_.subscribers_); + } + } + + void populate_subscriber_handles_with_node(rclcpp::node::Node &node) + { + // Use the number of subscriptions to pre allocate subscriber_handles + subscriber_handles_.subscriber_count_ = node.number_of_subscriptions_; + subscriber_handles_.subscribers_ = static_cast( + std::malloc(sizeof(void *) * node.number_of_subscriptions_)); + // Add subscriptions from groups + size_t handles_index = 0; + for (auto group : node.callback_groups_) + { + for (auto subscription : group->subscription_ptrs_) + { + assert(handles_index < node.number_of_subscriptions_); + subscriber_handles_.subscribers_[handles_index] = \ + subscription->subscription_handle_.data_; + handles_index += 1; + } + } + assert(handles_index == node.number_of_subscriptions_); + } + + void populate_subscriber_handles() + { + // Calculate the number of subscriptions and create shared_ptrs + size_t number_of_subscriptions = 0; + std::vector nodes; + bool has_invalid_weak_nodes = false; + for (auto &weak_node : weak_nodes_) + { + auto node = weak_node.lock(); + if (!node) + { + has_invalid_weak_nodes = false; + continue; + } + nodes.push_back(node); + number_of_subscriptions += node->number_of_subscriptions_; + } + // 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 pre allocate subscriber_handles + subscriber_handles_.subscriber_count_ = number_of_subscriptions; + 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."); + } + // Now fill the subscriber_handles with subscribers from the nodes + size_t handles_index = 0; + for (auto &node : nodes) + { + for (auto group : node->callback_groups_) + { + for (auto subscription : group->subscription_ptrs_) + { + assert(handles_index < number_of_subscriptions); + subscriber_handles_.subscribers_[handles_index] = \ + subscription->subscription_handle_.data_; + handles_index += 1; + } + } + } + assert(handles_index == number_of_subscriptions); + } + + struct AnyExecutable + { + AnyExecutable() : subscription(0), guard_condition_handle(0) {} + rclcpp::subscription::SubscriptionBase::SharedPtr subscription; + ros_middleware_interface::GuardConditionHandle *guard_condition_handle; + }; + + std::list get_ready_subscriber_handles() + { + std::list ready_subscriber_handles; + for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i) + { + void *handle = subscriber_handles_.subscribers_[i]; + if (!handle) + { + continue; + } + ready_subscriber_handles.push_back(handle); + } + return ready_subscriber_handles; + } + + 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 group : node->callback_groups_) + { + for (auto subscription : group->subscription_ptrs_) + { + if (subscription->subscription_handle_.data_ == subscriber_handle) + { + return subscription; + } + } + } + } + return rclcpp::subscription::SubscriptionBase::SharedPtr(); + } + + void remove_subscriber_handle_from_subscriber_handles_(void *handle) + { + for (size_t i = 0; i < subscriber_handles_.subscriber_count_; ++i) + { + if (handle == subscriber_handles_.subscribers_[i]) + { + // TODO: use nullptr here? + subscriber_handles_.subscribers_[i] = NULL; + } + } + } + + rclcpp::subscription::SubscriptionBase::SharedPtr get_next_subscription( + std::list &ready_subscriber_handles) + { + if (ready_subscriber_handles.size() == 0) + { + return rclcpp::subscription::SubscriptionBase::SharedPtr(); + } + while (ready_subscriber_handles.size() != 0) + { + void *handle = ready_subscriber_handles.front(); + ready_subscriber_handles.pop_front(); + remove_subscriber_handle_from_subscriber_handles_(handle); + auto subscription = get_subscription_by_handle(handle); + if (subscription) return subscription; + } + return rclcpp::subscription::SubscriptionBase::SharedPtr(); + } + + std::shared_ptr get_next_executable(bool nonblocking=false) + { + namespace rmi = ros_middleware_interface; + // Get any ready subscriber handles out of subscriber_handles_. + // TODO: operate directly on subscriber_handles_ or + // store ready_subscriber_handles to prevent rebuilding it every time + auto ready_subscriber_handles = get_ready_subscriber_handles(); + // If there are none + if (ready_subscriber_handles.size() == 0) + { + // Repopulate the subscriber handles and wait on them + reset_subscriber_handles(); + populate_subscriber_handles(); + // TODO: populate the guard handles correctly + rmi::GuardConditionHandles guard_condition_handles; + guard_condition_handles.guard_condition_count_ = 1; + guard_condition_handles.guard_conditions_ = static_cast( + std::malloc(sizeof(void *) * 1)); + guard_condition_handles.guard_conditions_[0] = \ + rclcpp::utilities::get_global_sigint_guard_cond().data_; + // Wait on the handles. + rmi::wait(subscriber_handles_, guard_condition_handles, nonblocking); + std::free(guard_condition_handles.guard_conditions_); + ready_subscriber_handles = get_ready_subscriber_handles(); + } + // At this point there should be something in either the subscriber handles + // Or in the guard handles. + // TODO: also check the timers here + std::shared_ptr any_exec(new AnyExecutable()); + // TODO: actually check the guard handles + any_exec->guard_condition_handle = 0; + // If any_exec.guard_condition_handle is valid, return now before checking + // for valid subscriptions. + if (any_exec->guard_condition_handle) + { + return any_exec; + } + // Check for subscriptions which are ready to be handled + any_exec->subscription = get_next_subscription(ready_subscriber_handles); + // If any_exec.subscription is valid, return now + if (any_exec->subscription) + { + return any_exec; + } + // If there is neither a ready guard condition nor subscription, return an + // empty any_exec anyways + return any_exec; + } + +private: + RCLCPP_DISABLE_COPY(Executor); + + std::vector> weak_nodes_; + ros_middleware_interface::SubscriberHandles subscriber_handles_; + +}; + +} /* executor */ +} /* rclcpp */ + +#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */ 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..777bda7 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp @@ -0,0 +1,68 @@ +/* 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 + +namespace rclcpp +{ +namespace executors +{ +namespace multi_threaded_executor +{ + +class MultiThreadedExecutor : public executor::Executor +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(MultiThreadedExecutor); + + void add_node(rclcpp::node::Node::SharedPtr &node_ptr) + { + this->weak_nodes_.push_back(node_ptr); + } + + void spin() + { + } + + void spin_some() + { + } + +private: + RCLCPP_DISABLE_COPY(MultiThreadedExecutor); + + std::vector> weak_nodes_; + +}; + +} /* 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..dbdb700 --- /dev/null +++ b/rclcpp/include/rclcpp/executors/single_threaded_executor.hpp @@ -0,0 +1,88 @@ +/* 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(); + if (any_exec->subscription) + { + // Do callback + execute_subscription(any_exec->subscription); + } + } + } + + void spin_node_some(rclcpp::node::Node &node) + { + reset_subscriber_handles(); + populate_subscriber_handles_with_node(node); + // non-blocking = true + auto any_exec = get_next_executable(true); + while (any_exec->subscription) + { + execute_subscription(any_exec->subscription); + // non-blocking = true + any_exec = get_next_executable(true); + } + reset_subscriber_handles(); + } + +private: + RCLCPP_DISABLE_COPY(SingleThreadedExecutor); + + std::vector> weak_nodes_; + +}; + +} /* 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..d96f5fa --- /dev/null +++ b/rclcpp/include/rclcpp/macros.hpp @@ -0,0 +1,42 @@ +/* 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)...); \ + } + +#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..181ae8a --- /dev/null +++ b/rclcpp/include/rclcpp/node.hpp @@ -0,0 +1,102 @@ +/* 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 + +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(); + + /* Creates a named callback group. */ + void + create_callback_group( + std::string group_name, + rclcpp::callback_group::CallbackGroupType callback_group_type); + + /* Return callback group if it exists, otherwise evaluates to false. */ + rclcpp::callback_group::CallbackGroup::SharedPtr + get_callback_group(std::string group_name); + + /* 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); + +private: + RCLCPP_DISABLE_COPY(Node); + + std::string name_; + + ros_middleware_interface::NodeHandle node_handle_; + + rclcpp::context::Context::SharedPtr context_; + + rclcpp::callback_group::CallbackGroup::SharedPtr default_callback_group_; + std::vector callback_groups_; + + size_t number_of_subscriptions_; + +}; + +} /* 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..8da4a58 --- /dev/null +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -0,0 +1,91 @@ +/* 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) +{ + node_handle_ = ::ros_middleware_interface::create_node(); + using rclcpp::callback_group::CallbackGroup; + using rclcpp::callback_group::CallbackGroupType; + default_callback_group_.reset(new CallbackGroup( + "default_group", CallbackGroupType::NonThreadSafe)); + callback_groups_.push_back(default_callback_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); +} + +template +typename subscription::Subscription::SharedPtr +Node::create_subscription( + std::string topic_name, + size_t queue_size, + std::function &)> callback) +{ + 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); + this->default_callback_group_->add_subscription(sub_base_ptr); + number_of_subscriptions_++; + return sub; +} + +#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..5a2e6a1 --- /dev/null +++ b/rclcpp/include/rclcpp/rate.hpp @@ -0,0 +1,115 @@ +/* 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 + +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; +}; + +template +class GenericRate : public RateBase +{ +public: + RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate); + + GenericRate(double rate) : rate_(rate), last_interval_(Clock::now()) + { + typedef std::chrono::nanoseconds nanoseconds; + auto tmp = std::chrono::duration(1.0f/rate_); + period_ = std::chrono::duration_cast(tmp); + } + + 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 + std::this_thread::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); + + double rate_; + 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..89abfe3 --- /dev/null +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -0,0 +1,68 @@ +/* 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 +#include + +namespace +{ + std::shared_ptr g_executor(0); + std::mutex g_executor_mutex; +} + +namespace rclcpp +{ + +using rclcpp::node::Node; +using rclcpp::rate::GenericRate; +using rclcpp::rate::WallRate; +using rclcpp::utilities::ok; +using rclcpp::utilities::init; + +void spin_some(Node &node) +{ + std::lock_guard lock(g_executor_mutex); + if (!g_executor) + { + // Create an executor + g_executor.reset(new rclcpp::executors::SingleThreadedExecutor()); + } + g_executor->spin_node_some(node); +} + +void spin_some(Node::SharedPtr &node_ptr) +{ + spin_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/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp new file mode 100644 index 0000000..200bf41 --- /dev/null +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -0,0 +1,64 @@ +/* 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_ + +#include + +#include + +#include +#include + +namespace +{ + volatile sig_atomic_t g_signal_status = 0; + ros_middleware_interface::GuardConditionHandle g_sigint_guard_cond_handle_; + + void signal_handler(int signal_value) + { + std::cout << "signal_handler(" << signal_value << ")" << std::endl; + g_signal_status = signal_value; + ros_middleware_interface::trigger_guard_condition(g_sigint_guard_cond_handle_); + } +} + +namespace rclcpp +{ +namespace utilities +{ + +void init(int argc, char *argv[]) +{ + ::g_sigint_guard_cond_handle_ = \ + ros_middleware_interface::create_guard_condition(); + std::signal(SIGINT, ::signal_handler); +} + +bool ok() +{ + return ::g_signal_status == 0; +} + +ros_middleware_interface::GuardConditionHandle get_global_sigint_guard_cond() +{ + return ::g_sigint_guard_cond_handle_; +} + +} /* namespace utilities */ +} /* namespace rclcpp */ + +#endif /* RCLCPP_RCLCPP_UTILITIES_HPP_ */