niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge pull request #1 from ros2/executors

Browse source 
Added support for executors
This commit is contained in:
William Woodall 2014-09-03 17:51:09 -07:00
commit bc7b340fc3
20 changed files with 1918 additions and 279 deletions
Download patch file Download diff file Expand all files Collapse all files

66
rclcpp/include/rclcpp/Node.h
View file

@ -1,66 +0,0 @@
#ifndef __rclcpp__Node__h__
#define __rclcpp__Node__h__
#include <memory>
#include <string>
#include <vector>
#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<typename ROSMessage>
Publisher<ROSMessage>* create_publisher(const char * topic_name)
{
const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name);
return new Publisher<ROSMessage>(publisher_handle);
}
template<typename ROSMessage>
Subscriber<ROSMessage>* create_subscriber(const char * topic_name, typename Subscriber<ROSMessage>::CallbackType callback)
{
const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = ::ros_middleware_interface::get_type_support_handle<ROSMessage>();
ros_middleware_interface::SubscriberHandle subscriber_handle = ::ros_middleware_interface::create_subscriber(node_handle_, type_support_handle, topic_name);
SubscriberInterface *sub = new Subscriber<ROSMessage>(subscriber_handle, std::string(topic_name), callback);
this->subscribers_.push_back(sub);
return static_cast<Subscriber<ROSMessage> *>(sub);
}
private:
ros_middleware_interface::NodeHandle node_handle_;
std::vector<SubscriberInterface *> subscribers_;
};
rclcpp::Node* create_node()
{
return new rclcpp::Node();
}
} /* namespace rclcpp */
#endif // __rclcpp__Node__h__

30
rclcpp/include/rclcpp/Publisher.h
View file

@ -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<typename ROSMessage>
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__

75
rclcpp/include/rclcpp/Subscriber.h
View file

@ -1,75 +0,0 @@
#ifndef __rclcpp__Subscriber__h__
#define __rclcpp__Subscriber__h__
#include <functional>
#include <string>
#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<typename ROSMessage>
class Subscriber : public SubscriberInterface
{
friend class rclcpp::Node;
public:
typedef std::function<void(const ROSMessage *)> 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__

75
rclcpp/include/rclcpp/callback_group.hpp Normal file
View file

@ -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 <atomic>
#include <string>
#include <vector>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
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::SubscriptionBase::SharedPtr> subscription_ptrs_;
std::vector<timer::TimerBase::SharedPtr> timer_ptrs_;
std::atomic_bool can_be_taken_from_;
};
} /* namespace callback_group */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_CALLBACK_GROUP_HPP_ */

44
rclcpp/include/rclcpp/context.hpp Normal file
View file

@ -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 <memory>
#include <rclcpp/macros.hpp>
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_ */

41
rclcpp/include/rclcpp/contexts/default_context.hpp Normal file
View file

@ -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 <rclcpp/context.hpp>
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_ */

589
rclcpp/include/rclcpp/executor.hpp Normal file
View file

@ -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 <iostream>
#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <list>
#include <memory>
#include <vector>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
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<rclcpp::node::Node> &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<AnyExecutable> 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<AnyExecutable> &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<void> 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<rclcpp::subscription::SubscriptionBase::SharedPtr> subs;
std::vector<rclcpp::timer::TimerBase::SharedPtr> 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<rclcpp::node::Node> 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<void **>(
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<void **>(
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<AnyExecutable> &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<AnyExecutable> &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<AnyExecutable>
get_next_ready_executable()
{
std::shared_ptr<AnyExecutable> 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<AnyExecutable>
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<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
typedef std::list<void*> SubscriberHandles;
SubscriberHandles subscriber_handles_;
typedef std::list<void*> GuardConditionHandles;
GuardConditionHandles guard_condition_handles_;
};
} /* executor */
} /* rclcpp */
#endif /* RCLCPP_RCLCPP_EXECUTOR_HPP_ */

108
rclcpp/include/rclcpp/executor/SingleThreadExecutor.h
View file

@ -1,108 +0,0 @@
#ifndef __rclcpp__executor__SingleThreadExecutor__h__
#define __rclcpp__executor__SingleThreadExecutor__h__
#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>
#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<void **>(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<rclcpp::Node *> nodes_;
};
} /* namespace executor */
} /* namespace rclcpp */
#endif // __rclcpp__executor__SingleThreadExecutor__h__

33
rclcpp/include/rclcpp/executors.hpp Normal file
View file

@ -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 <rclcpp/executors/multi_threaded_executor.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
namespace rclcpp
{
namespace executors
{
using rclcpp::executors::multi_threaded_executor::MultiThreadedExecutor;
using rclcpp::executors::single_threaded_executor::SingleThreadedExecutor;
}
}
#endif /* RCLCPP_RCLCPP_EXECUTORS_HPP_ */

112
rclcpp/include/rclcpp/executors/multi_threaded_executor.hpp Normal file
View file

@ -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 <cassert>
#include <cstdlib>
#include <memory>
#include <mutex>
#include <vector>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
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<std::thread> threads;
{
std::lock_guard<std::mutex> 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<AnyExecutable> any_exec;
{
std::lock_guard<std::mutex> 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_ */

67
rclcpp/include/rclcpp/executors/single_threaded_executor.hpp Normal file
View file

@ -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 <cassert>
#include <cstdlib>
#include <memory>
#include <vector>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/executor.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp/rate.hpp>
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_ */

44
rclcpp/include/rclcpp/macros.hpp Normal file
View file

@ -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 <memory> in the header when using this.
*/
#define RCLCPP_MAKE_SHARED_DEFINITIONS(Class) \
typedef std::shared_ptr<Class> SharedPtr; \
\
template<typename...Args> \
static std::shared_ptr<Class> \
make_shared(Args &&...args) \
{ \
return std::make_shared<Class>(std::forward<Args>(args)...); \
}
#define RCLCPP_INFO(Args) std::cout << Args << std::endl;
#endif /* RCLCPP_RCLCPP_MACROS_HPP_ */

120
rclcpp/include/rclcpp/node.hpp Normal file
View file

@ -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 <list>
#include <memory>
#include <string>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/context.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/subscription.hpp>
#include <rclcpp/timer.hpp>
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<typename MessageT> rclcpp::publisher::Publisher::SharedPtr
create_publisher(std::string topic_name, size_t queue_size);
/* Create and return a Subscription. */
template<typename MessageT>
typename rclcpp::subscription::Subscription<MessageT>::SharedPtr
create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> 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<long double, std::nano> period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group=nullptr);
typedef rclcpp::callback_group::CallbackGroup CallbackGroup;
typedef std::weak_ptr<CallbackGroup> CallbackGroupWeakPtr;
typedef std::list<CallbackGroupWeakPtr> 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_ */

165
rclcpp/include/rclcpp/node_impl.hpp Normal file
View file

@ -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 <memory>
#include <string>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <ros_middleware_interface/get_type_support_handle.h>
#include <rclcpp/contexts/default_context.hpp>
#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<typename MessageT>
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<MessageT>();
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 MessageT>
typename subscription::Subscription<MessageT>::SharedPtr
Node::create_subscription(
std::string topic_name,
size_t queue_size,
std::function<void(const std::shared_ptr<MessageT> &)> callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
namespace rmi = ::ros_middleware_interface;
auto &type_support_handle = rmi::get_type_support_handle<MessageT>();
auto subscriber_handle = rmi::create_subscriber(this->node_handle_,
type_support_handle,
topic_name.c_str());
using namespace rclcpp::subscription;
auto sub = Subscription<MessageT>::make_shared(subscriber_handle,
topic_name,
callback);
auto sub_base_ptr = std::dynamic_pointer_cast<SubscriptionBase>(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<long double, std::nano> period,
rclcpp::timer::CallbackType callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
return create_wall_timer(
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
callback,
group);
}
#endif /* RCLCPP_RCLCPP_NODE_IMPL_HPP_ */

58
rclcpp/include/rclcpp/publisher.hpp Normal file
View file

@ -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 <memory>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
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<typename MessageT> void
publish(std::shared_ptr<MessageT> &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_ */

120
rclcpp/include/rclcpp/rate.hpp Normal file
View file

@ -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 <chrono>
#include <memory>
#include <thread>
#include <rclcpp/macros.hpp>
#include <rclcpp/utilities.hpp>
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 Clock = std::chrono::high_resolution_clock>
class GenericRate : public RateBase
{
public:
RCLCPP_MAKE_SHARED_DEFINITIONS(GenericRate);
GenericRate(double rate)
: GenericRate<Clock>(
duration_cast<nanoseconds>(duration<double>(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<Clock> last_interval_;
};
typedef GenericRate<std::chrono::system_clock> Rate;
typedef GenericRate<std::chrono::steady_clock> WallRate;
}
}
#endif /* RCLCPP_RCLCPP_RATE_HPP_ */

75
rclcpp/include/rclcpp/rclcpp.hpp Normal file
View file

@ -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 <csignal>
#include <memory>
#include <rclcpp/node.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
{
constexpr std::chrono::seconds operator "" _s(unsigned long long s)
{
return std::chrono::seconds(s);
}
constexpr std::chrono::duration<long double> operator "" _s(long double s)
{
return std::chrono::duration<long double>(s);
}
constexpr std::chrono::nanoseconds
operator "" _ns(unsigned long long ns)
{
return std::chrono::nanoseconds(ns);
}
constexpr std::chrono::duration<long double, std::nano>
operator "" _ns(long double ns)
{
return std::chrono::duration<long double, std::nano>(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_ */

99
rclcpp/include/rclcpp/subscription.hpp Normal file
View file

@ -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 <functional>
#include <memory>
#include <string>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
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<void> create_message() = 0;
virtual void handle_message(std::shared_ptr<void> &message) = 0;
private:
RCLCPP_DISABLE_COPY(SubscriptionBase);
ros_middleware_interface::SubscriberHandle subscription_handle_;
std::string topic_name_;
};
template<typename MessageT>
class Subscription : public SubscriptionBase
{
public:
typedef std::function<void(const std::shared_ptr<MessageT> &)> 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<void> create_message()
{
return std::shared_ptr<void>(new MessageT());
}
void handle_message(std::shared_ptr<void> &message)
{
auto typed_message = std::static_pointer_cast<MessageT>(message);
callback_(typed_message);
}
private:
RCLCPP_DISABLE_COPY(Subscription);
CallbackType callback_;
};
} /* namespace subscription */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_SUBSCRIPTION_HPP_ */

124
rclcpp/include/rclcpp/timer.hpp Normal file
View file

@ -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 <chrono>
#include <functional>
#include <memory>
#include <thread>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
#include <rclcpp/macros.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp/utilities.hpp>
namespace rclcpp
{
// Forward declaration is for friend statement in GenericTimer
namespace executor {class Executor;}
namespace timer
{
typedef std::function<void()> 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 Clock = std::chrono::high_resolution_clock>
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<Clock>::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<Clock> loop_rate_;
};
typedef GenericTimer<std::chrono::steady_clock> WallTimer;
} /* namespace timer */
} /* namespace rclcpp */
#endif /* RCLCPP_RCLCPP_TIMER_HPP_ */

152
rclcpp/include/rclcpp/utilities.hpp Normal file
View file

@ -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 <iostream>
#include <cerrno>
#include <chrono>
#include <condition_variable>
#include <csignal>
#include <cstring>
#include <mutex>
#include <thread>
#include <ros_middleware_interface/functions.h>
#include <ros_middleware_interface/handles.h>
// 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<class Rep, class Period>
bool
sleep_for(const std::chrono::duration<Rep, Period>& sleep_duration)
{
// TODO: determine if posix's nanosleep(2) is more efficient here
std::unique_lock<std::mutex> 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_ */